ROS中按行读取Txt文本数据，C++

读取txt数据时，如果txt是多列或者数据格式不一样时，不太好用，所以，最好根据自己所要的内容，写一个保存程序，根据我自己的需要，在上一篇文章中写了订阅odom，按行保存到txt中的程序。 在这 ROS 中订阅odom消息并按行保存到txt中_宁缺毋滥灬的博客-CSDN博客

所以，这一篇程序根据保存的轨迹内容进行读取。

#include "ros/ros.h"  //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseStamped.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <visualization_msgs/MarkerArray.h>
#include <actionlib/client/simple_action_client.h>
#include <fstream>
//以下为串口通讯需要的头文件
#include <string>        
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include <sstream> //使用stringstream需要引入这个头文件
using namespace std;
/****************************************************************************/
using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

/*****************************************************************************/
int mark ;
int xp=0;
int yp=1;
int ox=2;
int oy=3;
int oz=4;
int ow=5;
int longth=0;
float txt1[1];
//actionlib_msgs::GoalStatusArray
void callback_result(const move_base_msgs::MoveBaseActionResult::ConstPtr& msg)//订阅/status主题回调函数
{
//actionlib_msgs::GoalStatusArray status_msg=*msg;
//move_base_msgs::MoveBaseActionResult status_msg=*msg;
actionlib_msgs::GoalStatus status;
//move_base_msgs::MoveBaseActionResult status;
status=msg->status;
//status=msg->status;
//mark=status_msg.status.status;
mark=status.status;

//ROS_INFO("2455");
//ROS_INFO("i heard: [%d]",left_com);   1正常This goal has been accepted by the simple action server，
//2 This goal was canceled because another goal was received by the simple action serve
//3到达目标 goal reached     4Failed to find a valid plan. Even after executing recovery behaviors

}

template <class Type>
Type stringToNum(const string& str)
{
	istringstream iss(str);
	Type num;
	iss >> num;
	return num;    
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "follow");//初始化串口节点
     ros::NodeHandle n;  //定义节点进程句柄
    // ros::Subscriber status_sub = n.subscribe("move_base/status", 5, callback_status);
   ros::Subscriber result_sub = n.subscribe("move_base/result", 5, callback_result);
 //ros::Publisher follow_pub= n.advertise<move_base_msgs::MoveBaseGoal>("move_base/goal", 20); //定义要发布/odom主题
  ros::Publisher follow_pub= n.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal", 5);
float x_pos,y_pos;
float orien_x,orien_y,orien_z,orien_w;
x_pos=0.0;
y_pos=0.0;
orien_x=0.0;
orien_y=0.0;
orien_z=0.0;
orien_w=0.0;
// send goal to move_base if we have something new to pursue
geometry_msgs::PoseStamped boal;
  //move_base_msgs::MoveBaseGoal goal;
geometry_msgs::Point target_position;
//target_position.x=x_pos;
//target_position.y=y_pos;
target_position.z=0.0;
 //boal.pose.position=target_position;
 //boal.pose.orientation.x = 0.0;
// boal.pose.orientation.y = 0.0;
 //boal.pose.orientation.z = 0.0;
// boal.pose.orientation.w = 1.;
 boal.header.frame_id = "map";
 boal.header.stamp = ros::Time::now();
 mark=5;
//
//读取txt路径数据
  ifstream fin;  
ifstream fine;
  string line;
 
fin.open("/home/~/point.txt");
fine.open("/home/~/point.txt");
//int longth=0;

if(fin) // 有该文件
    {//2
	//获取txt文件行数
	char c;
	while (fin.get(c))
	{
	if(c == '\n')
	longth++;
	}
	//cout << longth << endl;// longth是数据行数

	//获取txt每行内容
	const int txt_control_data_row = longth;//行
	//const int txt_control_data_col = 1;//列  v_c，oemag_c
	//string txt_control_data[txt_control_data_row][txt_control_data_col];
        //float txt_control_data[txt_control_data_row][txt_control_data_col];
	//float txt1[longth];
	int col=0;
	int row=0;
        while (getline (fine, line)) // line中不包括每行的换行符
        { //1
		
	if (row < longth) {
        	//if (col < 2) {
          //txt_control_data[row][col] = line;//stringToNum<double>(line);
	//txt_control_data[row][0] = stringToNum<double>(line);
	txt1[row]=stringToNum<double>(line);
		row++;
        	} 
	//else {
          //数据1,2写完一组，另起一行
          //col = 0;
          //row++;
         // txt_control_data[row][col] = stringToNum<double>(line);
        //	}
      //  col++;

	//}
   
         }//1
数据验证
/*
	for (int i = 0; i < 20; i++) {
 	cout << txt1[i] << " " //<< txt1[1]<< " " << txt1[2]<<" " << txt1[3]<<" " << txt1[4]
    //cout << i << " " << txt_control_data[i][0] << " " << txt_control_data[i][1]<< " " << txt_control_data[i][2]
         << " " << endl;
//经验证后，第0行对应第一行数据，第1 2行相等，对应第2行数据   x对应 0 3 +3 +3   y对应 1 4 +3 +3
  	}
*/验证end
    }//2
    else // 没有该文件
    {
        cout <<"no such file" << endl;
    }

//文件读取后输出txt1[], x对应 0 3 +3 +3   y对应 1 4 +3 +3
//x_pos=txt1[0],y_pos=txt[1]  +3
//int circle=1;
/
    while(ros::ok())
    {
//
/*
if(ow>=longth-7 && circle<=2)
{
	xp=0;
	yp=1;
	ox=2;
	oy=3;
	oz=4;
	ow=5;
circle++;
mark=5;
}
*//


	if(mark==5)
{
	x_pos=txt1[0];
	y_pos=txt1[1];
orien_x=txt1[2];
orien_y=txt1[3];
orien_z=txt1[4];
orien_w=txt1[5];
	target_position.x=x_pos;
	target_position.y=y_pos;
boal.pose.orientation.x = orien_x;
 boal.pose.orientation.y = orien_y;
 boal.pose.orientation.z = orien_z;
 boal.pose.orientation.w = orien_w;
	 boal.pose.position=target_position;

	follow_pub.publish(boal);
//cout << boal << endl;
//mark=0;
//ROS_INFO("5");
}
	if(mark==1)
	{
	ROS_INFO("1");
	mark=0;
	}
	if(mark==2)
	{
	ROS_INFO("2");
	mark=0;
	}

	if(mark==3)
	{
	xp=xp+7;
	yp=yp+7;
	ox=ox+7;
	oy=oy+7;
	oz=oz+7;
	ow=ow+7;
	x_pos=txt1[xp];
	y_pos=txt1[yp];
	orien_x=txt1[ox];
	orien_y=txt1[oy];
	orien_z=txt1[oz];
	orien_w=txt1[ow];
	target_position.x=x_pos;
	target_position.y=y_pos;
 boal.pose.orientation.x = orien_x;
 boal.pose.orientation.y = orien_y;
 boal.pose.orientation.z = orien_z;
 boal.pose.orientation.w = orien_w;
 	boal.pose.position=target_position;

	follow_pub.publish(boal);
	ROS_INFO("3");
  cout << x_pos << endl;

	mark=0;
	}
	if(mark==4)
	{
	xp=xp+7;
	yp=yp+7;
	ox=ox+7;
	oy=oy+7;
	oz=oz+7;
	ow=ow+7;
	x_pos=txt1[xp];
	y_pos=txt1[yp];
	orien_x=txt1[ox];
	orien_y=txt1[oy];
	orien_z=txt1[oz];
	orien_w=txt1[ow];
	
	target_position.x=x_pos;
	target_position.y=y_pos;
 boal.pose.orientation.x = orien_x;
 boal.pose.orientation.y = orien_y;
 boal.pose.orientation.z = orien_z;
 boal.pose.orientation.w = orien_w;
 	boal.pose.position=target_position;

	follow_pub.publish(boal);
	ROS_INFO("4");
	mark=0;
	}

	//follow_pub.publish(boal);
	ros::spinOnce();//周期执行
    }
	
}

fin.open("/home/~/point.txt");
fine.open("/home/~/point.txt");

同样，~，换成你自己的地址

作用就是读取你记录的轨迹，然后发布move_base/goal，指令，可以在RVIZ中选择点，进行跟踪，同时订阅了result消息，可以判断是否到达目标点

代码在这里https://download.csdn.net/download/weixin_42638797/85018826