C++读取rosbag中的位姿

#include <ros/ros.h>
#include "geometry_msgs/Vector3Stamped.h"
#include "geometry_msgs/QuaternionStamped.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <stdio.h>

using namespace std;
vector<double> pos_times;
vector<double> qua_times;
vector<double> p_x;
vector<double> p_y;
vector<double> p_z;
vector<double> q_x;
vector<double> q_y;
vector<double> q_z;
vector<double> q_w;

void PosCallback(const geometry_msgs::Vector3StampedConstPtr &pos_msg)
{
    double time = pos_msg->header.stamp.toSec();
    pos_times.push_back(time);
    p_x.push_back(pos_msg->vector.x);
    p_y.push_back(pos_msg->vector.y);
    p_z.push_back(pos_msg->vector.z);
    cout<<pos_msg->vector.x - 49.0099<<"  "<<pos_msg->vector.y - 8.41161<<"  "<<pos_msg->vector.z - 168.885<<"  "<<time<<endl;
}
void QuaCallback(const geometry_msgs::QuaternionStampedConstPtr &qua_msg)
{
    double time = qua_msg->header.stamp.toSec();
    qua_times.push_back(time);
    q_x.push_back(qua_msg->quaternion.x);
    q_y.push_back(qua_msg->quaternion.y);
    q_z.push_back(qua_msg->quaternion.z);
    q_w.push_back(qua_msg->quaternion.w);
}
void output_pos()
{
    int n=p_x.size();
    for(int i=0;i<n;i++)
    {
        cout<<pos_times[i]<<" "<<p_x[i]<<" "<<p_y[i]<<" "<<p_z[i]<<" "<<qua_times[i]<< " "
         <<q_x[i]<<" "<<q_y[i]<<" "<<q_z[i]<<endl;
    }
}


int main(int argc, char **argv)
{

    ros::init(argc, argv, "pos_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pos_sub = n.subscribe("/filter/positionlla", 1000, PosCallback);
    ros::Subscriber qua_sub = n.subscribe("/filter/quaternion", 1000, QuaCallback);
    // 循环等待回调函数
    output_pos();
    ros::spin();
    return 0;
}

cmake_minimum_required(VERSION 2.8.3)
project(pos_ext)

set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

find_package(catkin REQUIRED COMPONENTS
        roscpp
        )

include_directories(include ${catkin_INCLUDE_DIRS})

link_libraries(${catkin_LIBRARIES})

add_executable(extract_pos extract_pos.cpp)