1、解决的问题
多传感器数据融合的时候,由于各个传感器采集数据的频率的不同,例如odom 50Hz、Imu 100Hz、camera 25Hz,需要将传感器数据进行时间同步后才能进行融合。
融合的原理:
分别订阅不同的需要融合的传感器的主题,通过TimeSynchronizer 统一接收多个主题,并产生一个同步结果的回调函数,在回调函数里处理同步时间后的数据。
注意
只有多个主题都有数据的时候才可以触发回调函数。如果其中一个主题的发布节点崩溃了,则整个回调函数永远无法触发回调。
当多个主题频率一致的时候也无法保证回调函数的频率等于订阅主题的频率,一般会很低。实际测试订阅两个需要同步的主题,odom 50Hz、imu 50Hz,而回调函数的频率只有24Hz左右。
2、原文描述、
Time Synchronizer
See also: C++ message_filters::TimeSynchronizer API docs, Python message_filters.TimeSynchronizerThe 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.
Connections
InputC++: 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).
OutputC++: 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++)
Suppose you are writing a ROS node that needs to process data from two time synchronized topics. Your program will probably look something like this:
#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;
}