ros系统中写入和读取plan路径到文件

               salm无人车人ros系统中写入和读取plan路径到文件

   头文件

#include <ros/ros.h>
#include<ros/package.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <iostream>
#include <fstream>
#include <sys/stat.h>
#include <geometry_msgs/PoseStamped.h>

创建文件路径

    //创建文件路径
    void global_planner::createDirectory(const std::string& path)
    {
        struct stat info;

        if (stat(path.c_str(), &info) != 0)
        {
            // 如果路径不存在，则创建路径
            int status = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);

            if (status != 0)
            {
                std::cerr << "如果路径不存在，则创建路径 Failed to create directory: " << path << std::endl;
            }
        }
    }

写入文件

    //写入文件
    void global_planner::writeEulerAnglesToFile(const std::vector<geometry_msgs::PoseStamped>& plan, const std::string& filename)
    {
        std::ofstream outfile;
        outfile.open(filename.c_str());

        if (!outfile.is_open())
        {
            std::cerr << "写入文件 Failed to open file: " << filename << std::endl;
            return;
        }

        for (const auto& pose : plan)
        {
            tf2::Quaternion quat;
            tf2::fromMsg(pose.pose.orientation, quat);

            double roll, pitch, yaw;
            tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);

            outfile << pose.pose.position.x << " " << pose.pose.position.y << " "
                    << pose.pose.position.z << " " << yaw << std::endl;
        }

        outfile.close();

        std::cout << "写入文件 Euler angles have been saved to " << filename << std::endl;
    }

读取文件

    //读取文件
    std::vector<geometry_msgs::PoseStamped> global_planner::readEulerAnglesFromFile(const std::string& filename)
    {
        std::vector<geometry_msgs::PoseStamped> plan;
        std::ifstream infile(filename.c_str());
        if (!infile.is_open())
        {
            std::cerr << "Failed to open file: " << filename << std::endl;
            return plan;
        }

        geometry_msgs::PoseStamped pose;
        while (infile >> pose.pose.position.x >> pose.pose.position.y >> pose.pose.position.z)
        {
            double yaw;
            infile >> yaw;

            tf2::Quaternion quat;
            quat.setRPY(0.0, 0.0, yaw);

            pose.pose.orientation = tf2::toMsg(quat);
            plan.push_back(pose);
        }

        infile.close();

        return plan;
    }

保存路径到文件txt中

     if(isSavePlanToTxt_){
         //保存路径到文件txt中以便查看debug
         std::string filename = "plan.txt";
         std::string directory = fileOutPut_+"/planpose";
         createDirectory(directory);
         std::string filepath = directory + "/" + filename;
         writeEulerAnglesToFile(plan, filepath);
     }

读取txt中的path plan数据

      try{

                if(isSavePlanToTxt_){
                    std::string filename = fileOutPut_+"/planpose/plan.txt";
                    std::vector<geometry_msgs::PoseStamped> plan = readEulerAnglesFromFile(filename);
                    if (plan.empty())
                    {
                        std::cerr << "读取txt中的path plan数据Failed to read data from file: " << filename << std::endl;
                        std::string filenameMessage = "读取txt中的path plan数据Failed to read data from file: "+filename;
                        contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));

                        return 1;
                    }

                    std::cout << " 读取txt中的path plan数据 Successfully read " << plan.size() << " poses from file: " << filename << std::endl;
                    std::string filenameMessage = "读取txt中的path plan数据 Successfully read,poses from file: "+filename;
                    contnav_debug(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));
                    nav_msgs::Path gui_path;
                    publishPlan(plan,gui_path);
                    pathPlan_semiStructured_pub_.publish(gui_path);
                }else{
                    //+" ,isSendGoalPlanOverturn: "+(request.isSendGoalPlanOverturn?"true" : "false")
                    std::string filenameMessage = "读取txt中的path plan数据 Failed to read data from file,请确保 isSavePlanToTxt_参数为true，路径已经记录到 plan.txt文件中， isSavePlanToTxt_： "+isSavePlanToTxt_?"true":"false";
                    contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));
                }

            }catch (std::exception e){
                std::string filenameMessage = "exception 读取txt中的path plan数据Failed to read data from file: ";
                contnav_error(__FILE__,__LINE__,const_cast<char*>(filenameMessage.c_str()));
            }