#include<fstream>
#include<iostream>
using namespace std;
int main()
{
	ofstream out("D://wang//test.txt");//寫資料到指定目標資料夾，這個資料夾裡可以沒有test.txt檔案，
	if(out.is_open())
	{
		out<<"this is a line.\n";
		out<<"This is another lien.\n";
		out.close();
	}
	//從test.txt中讀取資料
	char buffer[256];
	ifstream in("D://wang//test.txt");
	if (!in.is_open())
	{
		cout<<"Error opening file";
	}
	while (!in.eof())
	{
		in.getline(buffer,100);
		cout<<buffer<<endl;
	}
	return 0;
 

實際應用

SmoothPath(std::vector<geometry_msgs::PoseStamped> &total_plan)

    {

	    
	     ofstream out1("/home/viewbot/wanglinearV5.txt",ios::app);
	    
	    int total_plan_size = total_plan.size();	    

	    double sum_x,sum_y;	
	    int d,window_0,window_1;

	    for (int i = 0; i <= total_plan_size; i++)

	    {

		    
	        double x1=total_plan[i].pose.position.x; 
	        double y1=total_plan[i].pose.position.y;
	          
		    sum_x = 0.0;

		    sum_y = 0.0;
		    d = 20;
            d = std::min(i, d);
            d = std::min(total_plan_size-i-1, d);
            window_0 = i-d;
            window_1 = i+d;		

		    for (int j = window_0; j < window_1+1; j++)

		    {

			    sum_x += total_plan[j].pose.position.x;

			    sum_y += total_plan[j].pose.position.y;

		    }

		    total_plan[i].pose.position.x = sum_x / (2*d+1);

		    total_plan[i].pose.position.y = sum_y / (2*d+1);
		     
	        out1<<x1<<' '<<y1<<' '<<total_plan[i].pose.position.x<<' '<<total_plan[i].pose.position.y<<endl; 

	    }	
	      out1.close();
	        

   }
 

畫圖

import matplotlib.pyplot as plt
filename = 'wanglinearV5.txt'
#filename = 'normalfootprint.txt'
x1,y1,x2,y2,e1,e2= [],[],[],[],[],[]


i=0


with open(filename, 'r') as f:#1
    lines = f.readlines()#2
    for line in lines:#3
        value = [float(s) for s in line.split(" ")]#4
        x1.append(value[0])
        y1.append(value[1])
        x2.append(value[2])
        y2.append(value[3])
        e1.append(value[0]-value[2])
        e2.append(value[1]-value[3])
        
        
       

def velotu():
    #line1=plt.scatter(x1, y1)
    line2=plt.plot(x1, y1)
    #line3=plt.scatter(x2, y2)
    line4=plt.plot(x2, y2)
    line5=plt.plot(e1, e2)
    plt.legend([line2, line4,line5], ["error_vel","error_ang","yy"], loc=1)
    
    plt.show()

if __name__ == '__main__':
 
    velotu()
    
 

平滑效果