#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
using namespace std;

void LoadImages(const string &trajectory, vector<string> &position_x)
{
    ifstream fTimes;
    // 資料流讀取檔案
    fTimes.open(trajectory.c_str());
    position_x.reserve(5000);
    //position_y.reserve(5000);
    //position_z.reserve(5000);
    //vTimeStamps.reserve(5000);
    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            //stringstream ss;
            //ss << s;
	        position_x.push_back(s);
            //double t;
            //ss >> t;
            //vTimeStamps.push_back(t/1e9);

        }
    }
}


int main(int argc, char **argv){
    if(argc != 2){
        cerr << endl << "Usage: wenjian" << endl;
    }
    vector<string> vposition;

    LoadImages(string(argv[1]),vposition);

    ros::init(argc,argv,"publish"); //初始化節點
    ros::NodeHandle n;
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("message",1000); //定義傳送端
    ros::Rate loop_rate(10); //頻率
    vector<string>::iterator i = vposition.begin();
    //auto i = vposition.begin();
    while (ros::ok()){
        if (i == vposition.end()){
            break;
        }
	std_msgs::String msg;
	std::stringstream ss;
        string s_out = *i;
	ss << s_out;
	msg.data = ss.str(); // 字串message
	chatter_pub.publish(msg);
        i = i + 1;
	ros::spinOnce();
	loop_rate.sleep();
	}
    return 0;
}