ros中的消息同步message_filter_message_filters::sync_polices::approximatetime

前言

A message filter is defined as something which a message arrives into and may or may not be spit back out of at a later point in time. An example is the time synchronizer, which takes in messages of different types from multiple sources, and outputs them only if it has received a message on each of those sources with the same timestamp.

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.

ExactTime Policy

必须是相同的时间戳才能同步上.

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

ApproximateTime Policy

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>

void callback(const sensor_msgs::ImageConstPtr& image1, const sensor_msgs::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<sensor_msgs::Image> image1_sub(nh, "image1", 1);
  message_filters::Subscriber<sensor_msgs::Image> image2_sub(nh, "image2", 1);

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

  ros::spin();
  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

注：void callback(const boost::shared_ptr&, …, const boost::shared_ptr&) , 回调函数的输入类型必须是ConstPtr类型的共享指针。

ApproximateTime Algorithm

Let S be the last published set, and T the next one being created. It works as follows:
1. When set S is published, for each topic, all messages older than the one in the set are discarded.
2.Messages are inserted in a topic-specific queue as they arrive.
3.Once each topic-specific queue contains at least one message, we find the latest message among the heads of the queues, that’s the pivot. The pivot belongs to every set contiguous to S, so it must belong to T (property d), hence its special status. For each message m older than the pivot, call Tm the smallest set that starts at m. It is obtained by taking the earliest message arrived after m on each queue. Since T starts at some message, it must be one of the Tm’s.
4.Go through messages m in order of time. If the next message cannot yet be determined, wait until the queues fill up enough.
5.When m reaches the pivot so that all Tm’s have been enumerated, or whenever it can be proved using various bounds that the smallest Tm has been found, publish the smallest Tm.

It works in three nested phases:

1.Find the pivot and a first valid candidate set.
2.Advance along the queues until one of the queues is empty, in search of a better candidate.
3.Advance past this point assuming that future messages will arrive at the most optimistic time, to try to prove that the current candidate is optimal.

Here are some example results. Time goes from left to right, each line is a topic, and each dot is a message. The red message is the pivot, and the broken line links the messages in a set.
我的理解:
1.对于每个需要同步的topic都有一个消息队列
2.如果每个队列中至少有一个msg, 选最后到的那个为pivot
3.计算每个队列中距离pivot最近的消息与pivot的消息差和, 为T
4.等待, 直到能够确定最小的T, 那么这个T set为需要publish的set
5.那么在这个publish set之前的消息都需要删除

实践

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
             message_filters)
1
2

头文件

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
1
2
3

boost::bind()

如果callback()函数是全局函数，如上例所示：

boost::bind(&callback, _1, _2)  
1

注： _1, _2为占位符， 代表接收的参数， 在函数真正调用的时候传入真正的参数。占位符的名字知识表示它在调用式中的顺序， 最多可以绑定9个占位符。
如果callback()函数是类成员函数：

boost::bind(&X::back, this, _1, _2)
1

注：第二个参数this指针指向当前对象， 从而可以调用当前对象的成员函数，因为this指针占用了一个占位符，所以最多绑定8个参数

message_filter作为成员变量

在构造函数中.

xx.h

class A
{
private:
typedef message_filters::sync_policies::ApproximateTime<type_a, type_b> MySyncPolicy;
typedef message_filters::Synchronizer<MySyncPolicy> Sync;
boost::shared_ptr<Sync> sync_;
message_filters::Subscriber<type_a> a_sub_;
message_filters::Subscriber<type_b> b_sub_;
};
1
2
3
4
5
6
7
8
9

xx.cpp
A::A(ros::NodeHandle& nh, ros::NodeHandle& priv_nh)
{
  a_sub_.subscribe(nh, a_topic, 100);
  b_sub_.subscribe(nh, b_topic, 100);
  sync_.reset(new Sync(MySyncPolicy(1000), a_sub_, b_sub_));
  sync_->registerCallback(boost::bind(&A::callback, this, _1, _2));
}
1
2
3
4
5
6
7
8

示例

图像和点云同步

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/CompressedImage.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

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

#include <pcl_conversions/pcl_conversions.h>
#include <opencv2/opencv.hpp>


void callback(const sensor_msgs::CompressedImageConstPtr input_image_msg,
              const sensor_msgs::PointCloud2ConstPtr input_cloud_msg)
{
  std_msgs::Header image_header = input_image_msg->header;
  std_msgs::Header cloud_header = input_cloud_msg->header;

  // sensor_msgs to cv image
  cv::Mat input_image;
  try
  {
    input_image = cv::imdecode(cv::Mat(input_image_msg->data), 1);
  }
  catch (cv_bridge::Exception& e)
  {
    std::cout << "could not convert sensor_msg image into opencv image!" << std::endl;
    return;
  }

  // sensor_msgs to point cloud
  pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud_ptr(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*input_cloud_msg, *input_cloud_ptr);
  if (input_cloud_ptr->size() == 0)
  {
    std::cout << "input cloud is empty, please check it out!" << std::endl;
    return;
  }

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "vision_node");
  ros::NodeHandle nh;
  ros::NodeHandle priv_nh("~");
  
  std::string camera_topic = "/camera_front/image_color/compressed";
  std::string lidar_topic = "/rslidar_points";
  message_filters::Subscriber<sensor_msgs::CompressedImage> camera_sub(nh, camera_topic, 10);
  message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh, lidar_topic, 10);
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::CompressedImage, sensor_msgs::PointCloud2>
      MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), camera_sub, lidar_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));
  ros::spin();

  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

多图像同步

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

#include <sensor_msgs/Image.h>

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

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

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

  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image1_sub, image2_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();
  return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28