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();
}