跳轉有道雲筆記
http://note.youdao.com/noteshare?id=202fd84e7ea246512824305e941b01c7
ROS中多傳感器時間同步
ROS官方教程---message_filter的使用
API鏈接：https://docs.ros.org/api/message_filters/html/c++/namespacemessage__filters.html
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訂閱者
訂閱者過濾器是對ROS訂閱的封裝，爲其他過濾器提供源代碼，訂閱者過濾器無法將另一個過濾器的輸出作爲其輸入，而是使用ROS主題作爲其輸入。
輸入輸出形式
輸入
無輸入連接
輸出
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 時間同步器
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;
}
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 緩存
用於存儲歷史時間的消息記錄。
給定消息流，最近的N個消息被緩存在環形緩衝器中，然後可以由客戶端檢索高速緩存的時間間隔。消息的時間戳從其頭字段確定。
如果消息類型不包含head，請參閱下面的解決方法。
緩存立即將消息傳遞到其輸出連接。
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;
}
1
如果某些消息的類型不包含頭字段，則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+]
鏈式過濾器允許您將多個單輸入/單輸出（簡單）過濾器動態鏈接在一起。 隨着添加過濾器，它們會按照添加的順序自動連接在一起。 它還允許您通過索引檢索添加的過濾器。
對於您要確定在運行時而不是編譯時應用哪些過濾器的情況，鏈最有用。
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);