ROS中的多传感器时间同步

问题

在对多传感器数据融合时，由于各个传感器采集数据的发布频率的不同，例如odom一般为50Hz、imu一般为100Hz、camera 一般为25Hz，需要将传感器数据进行时间同步后才能进行融合。

方法

分别订阅不同的需要融合的传感器的主题，通过TimeSynchronizer 统一接收多个主题，并产生一个同步结果的回调函数，在回调函数里处理同步时间后的数据。

输入

• C++: Up to 9 separate filters, each of which is of the form void callback(const boost::shared_ptr<M const>&). 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).

输出

• C++: For message types M0..M8, void callback(const boost::shared_ptr<M0 const>&, ..., const boost::shared_ptr<M8 const>&). 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.

示例

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

注意

1. 在示例中，只有当image_sub与info_sub都接收到话题的数据时，才会执行回调函数calback；
2. 即使当"image"与"camera_info"两个话题发布频率一致时，也不能保证回调函数callback的频率和他们一样，实际上会低很多。

更多

http://wiki.ros.org/message_filters