ROS之多个订阅数据同步

做传感器数据融合时，常常会需要用到多个数据，即需要同时订阅多个话题。那么，如何同步这些传感器数据的时间辍，并将它们放入一个回调函数中进行处理呢？

参考文档：

1. http://wiki.ros.org/message_filters
2. ROS自带多传感器时间同步机制Time Synchronizer测试
3. ros消息时间同步与回调

有些消息类型会带有一个头部数据结构，如下所示。信息中带有时间辍数据，可以通过这个数据进行时间同步。

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id

• 以下是一种同步的方式：Time Synchronizer

The TimeSynchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

#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 [ROS 1.1+]：

The Synchronizer filter synchronizes incoming channels by the timestamps contained in their headers, and outputs them in the form of a single callback that takes the same number of channels. The C++ implementation can synchronize up to 9 channels.

The Synchronizer filter is templated on a policy that determines how to synchronize the channels. There are currently two policies: ExactTime and ApproximateTime.

1. 当需要同步的所有消息都带有时间辍的头部数据：ExactTime

   The message_filters::sync_policies::ExactTime policy requires messages to have exactly the same timestamp in order to match. Your callback is only called if a message has been received on all specified channels with the same exact timestamp. The timestamp is read from the header field of all messages (which is required for this policy).

#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;
}

由于该同步策略是当所有需同步的话题的时间辍严格相等时，才会触发回调函数。这就会导致以下一些问题：

• 回调函数的触发频率必然小于等于这些话题中最小的发布频率；
• 回调函数的触发并不十分稳定，有时候甚至会出现长时间不被触发的情况。如下图所示，某一次的间隔甚至长达10s左右。

2. ROS提供了另外一种方法来实现数据的同步：ApproximateTime。与需要时间辍完全相同的ExactTime不同，该方法允许话题之间的时间辍存在一定的偏差。

   The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.

#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;
}

从下图可以看出，虽然该方法允许时间之间存在偏差，但实际上偏差并不大。而且比起上一种方法，这个方法的回调函数的触发频率快多了。

关于ApproximateTime，我还有一个不解的地方，这里做一下记录：

If not all messages have a header field from which the timestamp could be determined, see below for a workaround.
If some messages are of a type that doesn’t contain the header field, ApproximateTimeSynchronizer refuses by default adding such messages.

以上这两句话，似乎自相矛盾。不知道是不是我理解的问题。。。从时间同步的角度看，话题消息内容中应该必须要带上时间辍信息才能进行同步，但第一句话却说可以允许一些消息不带时间辍？

[补充于2021.2.11: 今天在使用ApproximateTime时同步了一个自定义的消息类型，发生了如下图所示的错误。后来查阅资料才发现是没有加header的原因，即没有时间辍，程序就无法根据时间进行同步。换句话说，该方法也是必须需要时间辍信息的。加上header后错误就没有了。]

另外需要注意的是，使用message_filters时，需要在CMakeLists.txt和package.xml中添加相关依赖：

# CMakeLists.txt
find_package( catkin REQUIRED COMPONENTS
	 ...
	message_filters
)
# package.xml
find_package( catkin REQUIRED COMPONENTS
	<build_depend>message_filters</build_depend>
	<build_export_depend>message_filters</build_export_depend>
	<exec_depend>message_filters</exec_depend>
)