ROS消息过滤器message_filters离线offline使用（读包时进行消息同步）

ROS message_filters提供了消息订阅并根据不同消息时间戳同步的功能，一般用来接收实时发布的rostopic，使用方法可以参见这篇博客。

本篇主要讨论一种扩展使用方法，即：当在离线场景使用rosbag c++ API读取ros包中的消息时，如何借用message_filters进行时间同步。

这一过程不涉及rostopic的发布，直接将从rosbag中读取的消息传递给callback函数。

直接上代码（不保证编译）：

#include <string>
#include <vector>
#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/synchronizer.h>
#include <sensor_msgs/Image.h>

/* 消息同步策略，此处选用ApproximateTime同步两个图像 */
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> imageSync;

/*
    直接将消息传递给message_filters主要用到message_filters::SimpleFilter中的signalMessage
    函数，由于signalMessage是protected成员函数，我们需要实现一个自己的subscriber类
    （继承message_filters::SimpleFilter）并提供一个public函数来调用signalMessage。
*/

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M> {
public:
    void newMessage(const boost::shared_ptr<M const> &msg) {
        this->signalMessage(msg);
    }
};

/*
    callback函数，用来处理同步的消息。
*/
void imageCallback(const sensor_msgs::ImageConstPtr &rgbMsg,
                   const sensor_msgs::ImageConstPtr &depthMsg) {
    ...
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "test_subscriber");
    std::string bagPath = "...";
    std::string rgbTopic = "...";
    std::string depthTopic = "...";
    
    /*
        使用我们自己的BagSubscriber创建两个订阅者，不需要提供消息名称。
        也可以使用new BagSubscriber<sensor_msgs::Image>()来初始化,取决于需求
    */
    BagSubscriber<sensor_msgs::Image> rgbSub;
    BagSubscriber<sensor_msgs::Image> depthSub;
    // 创建同步器，注册回调函数
    message_filters::Synchronizer<imageSync> sync(imageSync(10), rgbSub, depthSub);
    sync.registerCallback(boost::bind(&imageCallback, _1, _2));

    // 读取ros包,指定topic
    rosbag::Bag bag;
    bag.open(bagPath, rosbag::bagmode::Read);
    std::vector<std::string> topics;
    topics.push_back(rgbTopic);
    topics.push_back(depthTopic);
    rosbag::View views(bag, rosbag::TopicQuery(topics));
    // 遍历消息
    for (rosbag::MessageInstance const m : views) {
        if (m.getTopic() == rgbTopic) {
            sensor_msgs::Image::Ptr rgbMsg = m.instantiate<sensor_msgs::Image>();
            rgbSub.newMessage(rgbMsg);
        }
        else if (m.getTopic() == depthTopic) {
            sensor_msgs::Image::Ptr depthMsg = m.instantiate<sensor_msgs::Image>();
            depthSub.newMessage(depthMsg);
        }
    }

    bag.close();

}

本篇主要参考ROS官方教程， 用到的message_filters API参见这里。