message_filters_ros 消息 过滤

message_filters是一个关于消息过滤器的包，从ROS subscription或者其他filter接收消息，按照指定的策略输出消息。

message_filters::SimpleFilter< M >

定义接口的过滤器基类

Connection message_filters::SimpleFilter< M >::registerCallback( callback函数指针或函数对象 )
1

message_filters::Subscriber< M >

ROS1

API手册

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

//构造函数
template<class M>//消息类型 M
Subscriber (ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0);
//实现同样效果的subscribe函数
template<class M>
void message_filters::Subscriber< M >::subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints = ros::TransportHints(), ros::CallbackQueueInterface *callback_queue = 0 ) 	
1
2
3
4
5
6

典型应用：

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

message_filters::TimeSynchronizer时间同步器

ROS1

TimeSynchronizer过滤器通过包含在header中的时间戳来同步输入通道，以但个回调的形式输出。C++实现最多可以同步9个通道。

  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "image", 1);
  message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(nh, "camera_info", 1);
  TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> sync(image_sub, info_sub, 10);
  sync.registerCallback(boost::bind(&callback, _1, _2));
1
2
3
4

Policy-Based Synchronizer

Synchronizer filter以一个决定如何同步各输入通道消息的policy为模板参数。官方有两种现有的policy：ExactTime和ApproximateTime

ExactTime只有当消息的时间戳完全一样时才可以。

ApproximateTime Policy

源码

message_filters::sync_policies::ApproximateTime policy 使用自适应算法根据时间戳匹配消息。

ApproximateTime(uint32_t queue_size)
   : parent_(0)
   , queue_size_(queue_size)//存放的消息队列的长度
   , enable_reset_(false)
   , num_reset_deques_(0)
   , num_non_empty_deques_(0)
   , pivot_(NO_PIVOT)
   , max_interval_duration_(ros::DURATION_MAX)
   , age_penalty_(0.1)
   , has_dropped_messages_(9, false)
   , inter_message_lower_bounds_(9, ros::Duration(0))
   , warned_about_incorrect_bound_(9, false)
   , last_stamps_(9, ros::Time(0, 0)){}
void setAgePenalty(double age_penalty);//设置age_penalty_参数
void setInterMessageLowerBound(int i, ros::Duration lower_bound) {//单独为某个消息源设置inter_message_lower_bounds_参数
    inter_message_lower_bounds_[i] = lower_bound;
}
void setInterMessageLowerBound(ros::Duration lower_bound) {//为所有消息源设置相同的inter_message_lower_bounds_参数
    for (size_t i = 0; i < inter_message_lower_bounds_.size(); i++)
    {
        inter_message_lower_bounds_[i] = lower_bound;
    }
}
void setMaxIntervalDuration(ros::Duration max_interval_duration);//设置max_interval_duration_参数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

可选参数：

• Age penalty: 当比较后面的时间间隔会乘上一个惩罚因子(1+AgePenalty)，一个非零的Age penalty会更早的输出匹配消息集，或输出更多的消息集，以牺牲质量为代价
• inter message lower bound: 如果特定的消息两帧之间不能比已知的间隔更接近，给定这个间隔下界会帮助算法更早的得出一个给定消息集是最优匹配，从而减小延迟。不正确的下界将导致选择次优集合。一个典型的bound：相机帧率的一半。
• Max interval duration: 如果候选匹配消息集合的时间间隔大于此值则不考虑。丢弃最早的消息，转向下一个消息。

典型应用：

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
using namespace sensor_msgs;
void callback(const ImageConstPtr& image1, const ImageConstPtr& image2 ){}
int main(){
    message_filters::Subscriber<Image> image1_sub(nh, "image1", 1);
    message_filters::Subscriber<Image> image2_sub(nh, "image2", 1);
    typedef message_filters::sync_policies::ApproximateTime<Imamge, Image> SyncPolicy; 
    SyncPolicy mySyncPolicy(10);
    mySyncPolicy.setAgePenalty(0.1);
    mySyncPolicy.setInterMessageLowerBound( 0, ros::Duration(0.05) );
    mySyncPolicy.setInterMessageLowerBound( 0, ros::Duration(0.1) );
    mySyncPolicy.setMaxIntervalDuration( ros::Duration(0.2) )
    message_filters::Synchronizer<SyncPolicy> sync(mySyncPolicy, image1_sub, image2_sub);
    sync.registerCallback(boost::bind(&callback,_1,_2));    
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

tf2_ros::MessageFilter

API手册

是message_filters::SimpleFilter的tf2实现，用于对Stamped类型消息进行过滤，只有当有对应Stamp时刻的tf变换时才发出消息。

//构造函数
MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh);
template<class F>//message_filter类型
MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
: bc_(bc), queue_size_(queue_size), callback_queue_(nh.getCallbackQueue()) {
   init();
   setTargetFrame(target_frame);
   connectInput(f);
}
MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue);
template<class F>
MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue);
virtual void setQueueSize (uint32_t new_queue_size);//设置队列长度
void setTargetFrame (const std::string &target_frame);//设置目标坐标系
void setTolerance (const ros::Duration &tolerance);//设置时间容许误差
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

常用用法

#include <message_filters/subscriber.h>
#include <tf2_ros/message_filter.h>
message_filters::Subscriber< M > sub(nodehandle,topic,queue_size);
tf2_ros::MessageFilter< M > filter(sub, tf_buffer, target_frame, queue_size, nodehandle);
filter.registerCallback(callback函数);
1
2
3
4
5