rosbag的读写

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <nav_msgs/Path.h>

读写bag文件

open函数默认是读模式。

写bag文件：

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Write);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}
bag.write("record_plan",ros::Time::now(), coverage_plan);
bag.close();

打开文件的模式有：Write，Read，Append

读文件如下，bag文件只有一个话题/move_base/transformed_plan，一堆nav_msgs/Path消息

rosbag::Bag bag;
try{
    bag.open("/home/user/plan.bag", rosbag::bagmode::Read);
}
catch (const rosbag::BagException& ex) {
    ROS_ERROR("open rosbag error: %s", ex.what() );
    return;
}        
std::vector<std::string> topics;
bool bag_read_once = false;

topics.push_back(std::string("record_plan") );
rosbag::View view(bag, rosbag::TopicQuery(topics) );

BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
    nav_msgs::Path::ConstPtr i = m.instantiate<nav_msgs::Path>();
    plan_from_bag.header.frame_id = i->header.frame_id;
    plan_from_bag.header.stamp = i->header.stamp;
    plan_from_bag.header.seq = i->header.seq;

unsigned int path_size = i->poses.size();
    ROS_INFO("plan from bag size: %zu",i->poses.size() );
    plan_from_bag.poses.reserve(path_size);
    if(bag_read_once)    break;

plan_from_bag.poses = i->poses;
    bag_read_once = true;
}
bag.close();

注意读的时候，topics容器的元素必须还是number，与写文件时一致。如果不一致，读文件会不执行foreach

用到的类如下：

/* Create a view and add a query
 * param bag             The bag file on which to run this query
 * param query           The actual query to evaluate which connections to include
 * param start_time      The beginning of the time range for the query
 * param end_time        The end of the time range for the query
 * param reduce_overlap  If multiple views return the same messages, reduce them to a single message
 */
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
     ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);

class ROSBAG_DECL TopicQuery
{
public:
    TopicQuery(std::string const& topic);
    TopicQuery(std::vector<std::string> const& topics);

bool operator()(ConnectionInfo const*) const;

private:
    std::vector<std::string> topics_;
};

参考：
rosbag API