C++读取txt文件，并利用ROS将其作为数据流输出

秒速五厘米 2022-05-18 00:34 230阅读 0赞

#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;
    }