ROS从bag文件中读取相应的topic数据_Android

ROS从bag文件中读取相应的topic数据

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) { ..... } }