message_filters学习笔记

学习链接：http://wiki.ros.org/message_filters

message_filters

• 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.
• time synchronizer ---------------------the same timestamp.

Filter Pattern

• Outputs are connected through the registerCallback() method

Subscriber

The Subscriber filter is simply a wrapper around a ROS subscription that provides a source for other filters. The Subscriber filter cannot connect to another filter’s output, instead it uses a ROS topic as its input.

Connections

• Input

No input connections

• Output

C++: void callback(const boost::shared_ptr&) Python: callback(msg)

Example (C++)

message_filters::Subscriber<std_msgs::UInt32> sub(nh, "my_topic", 1);
sub.registerCallback(myCallback);

相当于

ros::Subscriber sub = nh.subscribe("my_topic", 1, myCallback);

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.

• TimeSynchronizer filter通过header 中包含的时间戳来同步 incoming channels，并以单个回调函数的形式输出，该回调函数需要相同数量的通道。C++的实现最多可以同步9个通道。

Connections

Input

C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr&). The number of filters supported is determined by the number of template arguments the class was created with. Python: N separate filters, each of which has signature callback(msg).

Output

C++: For message types M0…M8, void callback(const boost::shared_ptr&, …, const boost::shared_ptr&). The number of parameters is determined by the number of template arguments the class was created with. Python: callback(msg0… msgN). The number of parameters is determined by the number of template arguments the class was created with.

Example (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;
}

Cache

Stores a time history of messages.

Given a stream of messages, the most recent N messages are cached in a ring buffer, from which time intervals of the cache can then be retrieved by the client. The timestamp of a message is determined from its header field.
If the message type doesn’t contain a header, see below for workaround.
The Cache immediately passes messages through to its output connections.

Connections

Input

C++: void callback(const boost::shared_ptr&) Python: callback(msg)

Output

C++: void callback(const boost::shared_ptr&) Python: callback(msg)

In C++:

message_filters::Subscriber<std_msgs::String> sub(nh, "my_topic", 1);
message_filters::Cache<std_msgs::String> cache(sub, 100);
cache.registerCallback(myCallback);

In this example, the Cache stores the last 100 messages received on my_topic, and myCallback is called on the addition of every new message. The user can then make calls like cache.getInterval(start, end) to extract part of the cache.

If the message type does not contain a header field that is normally used to determine its timestamp, and the Cache is contructed with allow_headerless=True, the current ROS time is used as the timestamp of the message. This is currently only available in Python.

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.

Connections

Input

C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr&). The number of filters supported is determined by the number of template arguments the class was created with. Python: N separate filters, each of which has signature callback(msg).

Output

C++: For message types M0…M8, void callback(const boost::shared_ptr&, …, const boost::shared_ptr&). The number of parameters is determined by the number of template arguments the class was created with. Python: callback(msg0… msgN). The number of parameters is determined by the number of template arguments the class was created with.

ExactTime Policy

• The message_filters::sync_policies::ExactTime policy requires messages to have exactly the same timestamp in order to match.
• message_filters::sync_policies::ExactTime 要求message的时间戳必须相同
• 所有channels的具有相同时间戳的message都接收到
• Example (C++)

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

ApproximateTime Policy

• The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.
  If not all messages have a header field from which the timestamp could be determined, see below for a workaround.

• C++ Header: message_filters/sync_policies/approximate_time.h

• Example (C++)

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

If some messages are of a type that doesn’t contain the header field, ApproximateTimeSynchronizer refuses by default adding such messages. However, its Python version can be constructed with allow_headerless=True, which uses current ROS time in place of any missing header.stamp field:

import message_filters
from std_msgs.msg import Int32, Float32

def callback(mode, penalty):
  # The callback processing the pairs of numbers that arrived at approximately the same time

mode_sub = message_filters.Subscriber('mode', Int32)
penalty_sub = message_filters.Subscriber('penalty', Float32)

ts = message_filters.ApproximateTimeSynchronizer([mode_sub, penalty_sub], 10, 0.1, allow_headerless=True)
ts.registerCallback(callback)
rospy.spin()

message_filters::sync::ApproximateTime 重点关注

• 它能匹配具有不同时间戳的messages
• 我们把一组messages的size表示这组最早最晚时间戳的差值
• 它允许在某个epsilon时间差进行匹配， 与 ExactTime 不同
• 它满足这些特性：

1. 参数，不用指定epsilon时间差。可以提一些可选参数。
2. messages 只能被使用一次，两个组中不能共享message. 一些message可能被丢弃
3. 集合不交叉，都是一整段时间
4. 集合是连续的，集合之间没有空余的message
5. Sets are of minimal size among the sets contiguous to the previous published set. (没理解)
6. 输出只取决于时间戳，而不是message到达的时间

• 可选择的参数
  （剩下的详情见原网页吧，我暂时用不到了）请点击链接