#include <iostream>

#include <stdio.h>

#include <fstream>

#include <queue>

#include <mutex>

    std::string gpssavename="/home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin/src/VINS-Fusion/outdata/vio_global.csv";

    std::ofstream foutC(gpssavename, ios::app);

    foutC.setf(ios::fixed, ios::floatfield);

    foutC.precision(0);

    foutC << pose_msg->header.stamp.toSec() << " ";

    foutC.precision(5);

    foutC   << global_t.x() << " "

            << global_t.y() << " "

            << global_t.z() << " "

            << global_q.w() << " "

            << global_q.x() << " "

            << global_q.y() << " "

            << global_q.z() << endl;

    foutC.close();