路径path保存到文件中，std::vector＜geometry_msgs::PoseStamped＞ plan集合中的坐标数据保存到 txt文件中

1.std::vector<geometry_msgs::PoseStamped> plan集合中的坐标数据，4元素转成欧拉角保存到 txt文件中

#include <iostream>
#include <fstream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>

void 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)
    {
        double roll, pitch, yaw;
        tf::Quaternion quat;
        tf::quaternionMsgToTF(pose.pose.orientation, quat);
        tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

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

    outfile.close();
}

int main()
{
    // 假设已经读取到了 plan 集合，保存在 std::vector<geometry_msgs::PoseStamped> plan 中
    std::vector<geometry_msgs::PoseStamped> plan;

    std::string filename = "euler_angles.txt";
    writeEulerAnglesToFile(plan, filename);

    std::cout << "Euler angles have been saved to " << filename << std::endl;

    return 0;
}

2.std::vector<geometry_msgs::PoseStamped> plan集合中的坐标数据，4元素转成欧拉角保存到 txt文件中

#include <iostream>
#include <fstream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

void 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();
}

int main()
{
    // 假设已经读取到了 plan 集合，保存在 std::vector<geometry_msgs::PoseStamped> plan 中
    std::vector<geometry_msgs::PoseStamped> plan;

    std::string filename = "euler_angles.txt";
    writeEulerAnglesToFile(plan, filename);

    std::cout << "Euler angles have been saved to " << filename << std::endl;

    return 0;
}

3.从文件中euler_angles.txt读取 坐标与欧拉角，把数据读取到这个std::vector<geometry_msgs::PoseStamped> plan集合中；

#include <iostream>
#include <fstream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

std::vector<geometry_msgs::PoseStamped> 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;
}

int main()
{
    std::string filename = "euler_angles.txt";

    std::vector<geometry_msgs::PoseStamped> plan = readEulerAnglesFromFile(filename);

    if (plan.empty())
    {
        std::cerr << "Failed to read data from file: " << filename << std::endl;
        return 1;
    }

    std::cout << "Successfully read " << plan.size() << " poses from file: " << filename << std::endl;

    return 0;
}

在 readEulerAnglesFromFile 函数中，我们首先尝试打开文件。如果打开文件失败，我们将输出错误信息并返回一个空的计划（std::vector<geometry_msgs::PoseStamped>）。否则，我们将从文件中读取每个位姿的位置和欧拉角，并将其转换为四元数形式的姿态，并将其存储到计划向量中。

在 main 函数中，我们将读取文件中的数据，并将其存储到 std::vector<geometry_msgs::PoseStamped> 集合中。如果无法读取数据，则输出错误信息并返回 1。否则，我们将输出成功读取的位姿数量。

请注意，在 readEulerAnglesFromFile 函数中，我们假设输入文件的格式如下：

x1 y1 z1 yaw1
x2 y2 z2 yaw2
x3 y3 z3 yaw3
...

其中，每行包含一个位置（x，y，z）和一个 yaw 角度。