使用c++读取posestamped格式的路径点

好的，对于C++中的ROS程序，您可以使用ROS消息中的 geometry_msgs/PoseStamped 来读取路径点。下面是一个示例代码：

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <vector>

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "read_path_points");
    ros::NodeHandle nh;

    // 订阅路径点
    ros::Subscriber sub = nh.subscribe("/path_points", 1, pathPointsCallback);

    // 循环读取ROS消息
    ros::spin();

    return 0;
}

void pathPointsCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // 读取路径点
    std::vector<geometry_msgs::PoseStamped> path_points;
    path_points.push_back(*msg);

    // 打印路径点
    for (auto point : path_points)
    {
        ROS_INFO("Position: x=%f, y=%f, z=%f", point.pose.position.x, point.pose.position.y, point.pose.position.z);
        ROS_INFO("Orientation: x=%f, y=%f, z=%f, w=%f", point.pose.orientation.x, point.pose.orientation.y, point.pose.orientation.z, point.pose.orientation.w);
    }
}

其中，"/path_points" 是您的路径点主题名称，pathPointsCallback 函数用于处理读取的路径点消息。在该函数中，您可以将读取到的路径点存储到一个 std::vector<geometry_msgs::PoseStamped> 中，并进行后续处理。