void readDataFromBag(const std::string &bag_name, const std::string &amcl_topic_name, const std::string &wheel_topic_name) { rosbag::Bag bag; try { bag.open(bag_name, rosbag::bagmode::Read); } catch (std::exception& e) { std::cout << "ERROR!" << std::endl; exit(-1); } std::vector<std::string> topics; topics.push_back(wheel_topic_name); topics.push_back(amcl_topic_name); std::cout<<topics.at(0)<<topics.at(1)<<std::endl; rosbag::View view(bag, rosbag::TopicQuery(topics.at(0)));//讀取nav_msgs::odometry格式的輪式里程計數據 for (rosbag::MessageInstance const m: view) { nav_msgs::OdometryConstPtr odom_wheel=m.instantiate<nav_msgs::Odometry>(); if(odom_wheel!=NULL) { ...... } } rosbag::View view1(bag, rosbag::TopicQuery(topics.at(1)));//讀取nav_msgs::odometry格式的amcl里程計數據 { for(rosbag::MessageInstance const m:view1) { nav_msgs::OdometryConstPtr odom_amcl=m.instantiate<nav_msgs::Odometry>(); if(odom_amcl!=NULL) { ..... } } } bag.close(); return; } int main(int argc,char **argv) { ros::init(argc,argv,"data_analyse"); ros::NodeHandle nh("~"); std::string file_path; std::string bagname; std::string odom_wheel_topic; std::string odom_amcl_topic; ros::param::get("~file_path", file_path); ros::param::get("~bagname", bagname); ros::param::get("~odom_wheel_topic", odom_wheel_topic); ros::param::get("~odom_amcl_topic", odom_amcl_topic); readDataFromBag(bagfile,odom_amcl_topic,odom_wheel_topic); std::string bagfile=file_path+bagname+".bag"; ros::spin(); return 0;
以上簡單地寫了個從bag文件中讀取兩個topic的軟件框架,若是有多個topic要讀,最好把rosbag viewer寫成vector容器遍歷的形式。框架
rosbag::View view(bag, rosbag::TopicQuery(topics); foreach(rosbag::MessageInstance const m:view) { nav_msgs::OdometryConstPtr odom_wheel=m.instantiate<nav_msgs::Odometry>(); if(odom_wheel!=NULL) { ...... } nav_msgs::OdometryConstPtr odom_amcl=m.instantiate<nav_msgs::Odometry>(); if(odom_amcl!=NULL) { ..... } }