消息同步器的使用方法 message_filter的使用

这篇博客转自https://blog.csdn.net/chishuideyu/article/details/77479758

ROS官方教程[翻译]---message_filter的使用

翻译 2017年08月22日 14:26:23

标签：
算法 /
ros学习

1 综述

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

2 过滤器模式

为了提供统一的接口与输出，message_filters中所有的消息过滤器都遵循着相同的模式连接输入和输出。
输入是通过过滤器构造函数或者是connectInput()方法链接。
输出是通过registerCallback()方法连接。
但是每个过滤器都定义了输入和输出的类型，所以并不是所有的过滤器都可以直接相互连接。
例如，给出两个过滤器FooFilter和BarFilter，其中FooFilter的输出作为BarFilter的输入，将foo连接到bar可以是（在C ++中）：

FooFilter foo;
BarFilter bar(foo);//通过过滤器构造函数

或者是

FooFilter foo;
BarFilter bar;
bar.connectInput(foo);//connectInput()方法链接

在Python中

bar(foo)

或

bar.connectInput(foo)

然后接着讲bar输出到你所指定的回调函数中：

bar.registerCallback(myCallback);

2.1 registerCallback()

你可以使用registerCallback()方法注册多个回调，他们将会按照注册的顺序依次进行调用。
在C++中，registerCallback()返回一个message_filters :: Connection对象，允许您通过调用其disconnect()方法断开回调。如果您不想手动断开回调，则不需要存储此连接对象。

3 Subscriber订阅者

C++ message_filters::Subscriber API docs
Python message_filters.Subscriber

订阅者过滤器是对ROS订阅的封装，为其他过滤器提供源代码，订阅者过滤器无法将另一个过滤器的输出作为其输入，而是使用ROS主题作为其输入。

3.1 输入输出形式

输入
无输入连接
输出
C++: void callback(const boost::shared_ptr<M const>&)
Python: callback(msg)

3.2 例子(C++)

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

同等于

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

3.3 例子(Python)

sub = message_filters.Subscriber("pose_topic", robot_msgs.msg.Pose)
sub.registerCallback(myCallback)

4. Time Synchronizer 时间同步器

C++ message_filters::TimeSynchronizer API docs
Python message_filters.TimeSynchronizer
TimeSynchronizer过滤器通过包含在其头中的时间戳来同步输入通道，并以单个回调的形式输出它们需要相同数量的通道。 C ++实现可以同步最多9个通道。

4.1 输入输出形式

输入
C ++：　最多9个独立的过滤器，每个过滤器的形式为void callback（const boost :: shared_ptr <M const>＆）。支持的过滤器数量由类创建的模板参数的数量决定。
Python：　N个单独的过滤器，每个过滤器都有签名回调（msg）。

输出
C ++：对于消息类型M0..M8，void callback（const boost :: shared_ptr <M0 const>＆，...，const boost :: shared_ptr <M8 const>＆），参数的数量由类创建的模板参数的数量决定。
Python： callback（msg0 .. msgN）。参数的数量由类创建的模板参数的数量决定。

4.2 例子(C++)

假设您正在编写一个需要从两个时间同步主题处理数据的ROS节点。您的程序可能看起来像这样：

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

在这种特殊情况下，您可以使用image_transport中的CameraSubscriber类，它基本上封装了上面的过滤代码。

4.3 例子(Python)

import message_filters
from sensor_msgs.msg import Image, CameraInfo

def callback(image, camera_info):
  # Solve all of perception here...

image_sub = message_filters.Subscriber('image', Image)
info_sub = message_filters.Subscriber('camera_info', CameraInfo)

ts = message_filters.TimeSynchronizer([image_sub, info_sub], 10)
ts.registerCallback(callback)
rospy.spin()

5. Time Sequencer 时间序列

C++ message_filters::TimeSequencer API docs
Python版的时间序列过滤器还未实现

TimeSequencer过滤器根据报头的时间戳保证按时间顺序调用消息。TimeSequencer构造有一个特定的延迟，它指定在传递消息之前排队多长时间。消息的回调直到消息的时间戳到达一定时间节点的时候才会调用。然而，对于所有已经至少延迟的消息，它们的回调被调用并保证是按时间顺序的。如果一条消息从已经有其回调调用的消息之前的某个时间到达，则会被丢弃。

5.1 输入输出格式

输入
C++: void callback(const boost::shared_ptr<M const>&)
输出
C++: void callback(const boost::shared_ptr<M const>&)

5.2 例子(C++)

C ++版本采用延迟更新速率。更新速率决定了定序器对准备好通过的消息检查其队列的频率。最后一个参数是在开始抛出一些消息之前排队的消息数。

message_filters::Subscriber<std_msgs::String> sub(nh, "my_topic", 1);
message_filters::TimeSequencer<std_msgs::String> seq(sub, ros::Duration(0.1), ros::Duration(0.01), 10);
seq.registerCallback(myCallback);

6. Cache 缓存

C++ message_filters::Cache API docs
Python message_filters.Cache

用于存储历史时间的消息记录。
给定消息流，最近的N个消息被缓存在环形缓冲器中，然后可以由客户端检索高速缓存的时间间隔。消息的时间戳从其头字段确定。
如果消息类型不包含标题，请参阅下面的解决方法。
缓存立即将消息传递到其输出连接。

6.1 输入输出形式

输入
C++: void callback(const boost::shared_ptr<M const>&) Python: callback(msg)
输出
C++: void callback(const boost::shared_ptr<M const>&) Python: callback(msg)

在Ｃ＋＋中：

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

在Python中

sub = message_filters.Subscriber('my_topic', sensor_msgs.msg.Image)
cache = message_filters.Cache(sub, 100)

在这个例子中，Cache存储了从my_topic上接收的最后100条消息，并且在添加每条新消息时调用myCallback。然后，用户可以调用cache.getInterval（start，end）来提取部分缓存。

如果消息类型不包含通常用于确定其时间戳的头字段，并且缓存与allow_headerless = True相关联，则将当前ROS时间用作消息的时间戳。目前仅在Python中可用。

sub = message_filters.Subscriber('my_int_topic', std_msgs.msg.Int32)
cache = message_filters.Cache(sub, 100, allow_headerless=True)
# the cache assigns current ROS time as each message's timestamp

7. Policy-Based Synchronizer 基于策略的同步器 [ROS 1.1+]

Synchronizer filter同步过滤器通过包含在其头中的时间戳来同步输入通道，并以单个回调的形式输出它们需要相同数量的通道。 C ++实现可以同步最多9个通道。

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

C++ Header: message_filters/synchronizer.h

7.1 输入输出形式

输入
C ++：最多9个独立的过滤器，每个过滤器的形式为void callback（const boost :: shared_ptr <M const>＆）。支持的过滤器数量由类创建的模板参数的数量决定。
Python： N个单独的过滤器，每个过滤器都有签名回调（msg）。

输出
C ++：对于消息类型M0..M8，void callback（const boost :: shared_ptr <M0 const>＆，...，const boost :: shared_ptr <M8 const>＆）。参数的数量由类创建的模板参数的数量决定。
Python： callback（msg0 .. msgN）。参数的数量由类创建的模板参数的数量决定。

7.2 ExactTime策略

message_filters :: sync_policies :: ExactTime策略要求消息具有完全相同的时间戳以便匹配。只有在具有相同确切时间戳的所有指定通道上收到消息时，才会调用回调。从所有消息的头域读取时间戳（这是该策略所必需的）。
C++头文件：message_filters/sync_policies/exact_time.h

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

7.3 ApproximateTime 策略

message_filters :: sync_policies :: ApproximateTime策略使用自适应算法来匹配基于其时间戳的消息。
如果不是所有的消息都有一个标题字段，从中可以确定时间戳，请参见下面的解决方法。
C++头文件：message_filters/sync_policies/approximate_time.h
例子(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;
}

如果某些消息的类型不包含头字段，则ApproximateTimeSynchronizer默认拒绝添加此类消息。但是，它的Python版本可以用allow_headerless = True来构造，它使用当前的ROS时间代替任何缺少的header.stamp字段：

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()

8. Chain 链[ROS 1.1+]

C++ API Docs

链式过滤器允许您将多个单输入/单输出（简单）过滤器动态链接在一起。随着添加过滤器，它们会按照添加的顺序自动连接在一起。它还允许您通过索引检索添加的过滤器。

对于您要确定在运行时而不是编译时应用哪些过滤器的情况，链最有用。

8.1. 输入输出形式

输入
C++: void callback(const boost::shared_ptr<M const>&)
输出
C++: void callback(const boost::shared_ptr<M const>&)

8.2 例子(C++)

void myCallback(const MsgConstPtr& msg)
{
}

Chain<Msg> c;
c.addFilter(boost::shared_ptr<Subscriber<Msg> >(new Subscriber<Msg>));
c.addFilter(boost::shared_ptr<TimeSequencer<Msg> >(new TimeSequencer<Msg>));
c.registerCallback(myCallback);

裸指针
可以传递裸指针。当链式被破坏时，这些不会被自动删除。

Chain<Msg> c;
Subscriber<Msg> s;
c.addFilter(&s);
c.registerCallback(myCallback);

检索过滤器

Chain<Msg> c;
size_t sub_index = c.addFilter(boost::shared_ptr<Subscriber<Msg> >(new Subscriber<Msg>));
boost::shared_ptr<Subscriber<Msg> > sub = c.getFilter<Subscriber<Msg> >(sub_index);