ROS多消息同步回调机制

1、需求起因

一般情况下，很少需要同步订阅多个话题，只需要订阅单个话题实现逻辑处理即可。但最近需要研究一个端到端的图像信息和控制输出的神经网络的学习，因此需要将三个图像和控制输出进行一个同步回调。所以才有了同步回调的需求，下面是此次需求解决的小笔记。

2、同步回调机制——Time Synchronizer

Time Synchronizer根据多个传入消息的时间戳进行同步，从而实现调用同一个回调函数的目的。下面是ROS wiki的一个例子：

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

关键步骤

1. 建立需要订阅的消息对应的订阅器

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

2. 建立同步器

   TimeSynchronizer<Image, CameraInfo> sync(image_sub, info_sub, 10);

3. 为同步器设置回调函数

   sync.registerCallback(boost::bind(&callback, _1, _2));

4. 构建多消息回调函数

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

3、Policy-Based Synchronizer

另外，ROS还有一种Policy-Based的消息同步机制，本质与上述Time Synchronizer类似。它分为两种，ExactTime Policy 和ApproximateTime Policy。

3.1 ExactTime 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;
}

3.2 ApproximateTime Policy

这种方法根据输入消息的时间戳进行近似匹配，不要求消息时间完全相同。

代码实现：

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