在ros中利用C++订阅话题输出为TXT文本及从文本读取发布话题

订阅话题输出TXT文本

/home/zero/catkin_ws/src/tang/src/save_point.cpp

rosrun tang save_point

#include "ros/ros.h"
#include <geometry_msgs/PointStamped.h>
#include <fstream>
#include <iostream>
#include <Eigen/Core>
#include <iomanip>
using namespace std;

void subCallback(const geometry_msgs::PointStamped &msg)
{
   Eigen::Vector3d way_points;
   way_points(0)  = msg.point.x * 100 / 100;
   way_points(1)  = msg.point.y * 100 / 100;
   way_points(2)  = msg.point.z * 100 / 100;
//    double x = msg.point.x * 100 / 100;
//    double y = msg.point.y * 100 / 100;
//    double z = msg.point.z * 100 / 100;

   std::ofstream out("/home/zero/catkin_ws/src/tang/point.txt",std::ios::app);
   //<<fixed<<setprecision(2)的作用就是将输出结果转化为小数点后两位 
   // out<<fixed<<setprecision(2)<<way_points(0)<<"\t"<<way_points(1)<<"\t"<<way_points(2)<<std::endl;
   out<<fixed<<setprecision(2)<<way_points(0)<<"\t\t"<<way_points(1)<<"\t\t"<<way_points(2)<<std::endl; 
   // out<<fixed<<setprecision(2)<<x<<" "<<y<<" "<<z<<std::endl;
   out.close();
}
 
int main(int argc, char **argv){

    ros::init(argc,argv,"subscrible_01"); //初始化节点
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("clicked_point", 1000, subCallback);
    
    ros::spin ();
    return 0;
}

读取txt文本发布话题

/home/zero/catkin_ws/src/tang/src/read_point02.cpp

rosrun tang read_point02

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <geometry_msgs/PointStamped.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);

    while(!fTimes.eof())
    {
        string s;
        getline(fTimes,s);
        if(!s.empty())
        {
            // stringstream ss;
            // ss << s;
            //ROS_INFO("%s",s.c_str());
	        position_x.push_back(s); 
        }
    }
}
 
 
int main(int argc, char **argv){

    vector<string> vposition;
 
    // LoadImages(string(argv[1]),vposition);
    LoadImages("/home/zero/catkin_ws/src/tang/point.txt",vposition);
 
    ros::init(argc,argv,"publish_02"); //初始化节点
    ros::NodeHandle n; 
    ros::Publisher chatter_pub = n.advertise<geometry_msgs::PointStamped>("clicked_point",1); //定义发送端
    ros::Rate loop_rate(10); //频率
    //vector<string>::iterator i = vposition.begin();
    //auto i = vposition.begin();

    geometry_msgs::PointStamped msg;
    msg.header.frame_id = "map";
    // int num = vposition.size();

    // printf("num = %d \n",num);
    int i = 0;
    while (ros::ok()){
        // if (i == (vposition.end() - 5)){
        if (i > (vposition.size() - 1)){
            break;
        }


        string x, y, z;
    
        // for (int j = 0; j < vposition.size(); j++)
        // {
        x = vposition[i].substr(0, 5); //substr就是截取字符，20指的是在第几个字符上，13从第20个数据向后截取的长度。
        
        msg.point.x = atof(x.c_str());
        //X.push_back(atof(x.c_str())); //string类型转化为double型分为两步，先将string转为字符数组，然后数组转为double。
                                    //c_str将string转为数组，atof将数组转为double。
        y = vposition[i].substr(6, 11);
        msg.point.y = atof(y.c_str());
        
        msg.point.z = 0;

        chatter_pub.publish(msg);

        ROS_INFO("i = %d \n",i);
        i = i + 1;    
        ros::spinOnce();
        loop_rate.sleep();
	}
    return 0;
}

读取效果

参考文献：

C / C++ 保留两位小数（setprecision(n)的一些用法总结）：

https://blog.csdn.net/qq_36667170/article/details/79265224

C++读取txt文件的行数据和列数据：

https://blog.csdn.net/weixin_40224801/article/details/105207642

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

https://blog.csdn.net/yg838457845/article/details/81232169

C++如何在文件中保存数据并对齐？

https://blog.csdn.net/qq_34107003/article/details/107531724

ROS学习 Python读写文本文件：

https://www.cnblogs.com/TIANHUAHUA/p/8443973.html

ros中c++输出gps坐标为txt文件：

https://blog.csdn.net/weixin_41621060/article/details/108319923