如何读取nav_msgs::Path中的信息？

Question

想写一个可以从nav_msgs::Path格式的话题中读取信息的回调函数程序，不会如何处理订阅到的信息，恳请指点

Answer 1

void pathCallback(const nav_msgs::Path::ConstPtr& msg)
{
    nav_msgs::Path path = *msg;
    // 在这里处理path消息
}
void pathCallback(const nav_msgs::Path::ConstPtr& msg)
{
    nav_msgs::Path path = *msg;

    // 遍历poses数组，获取机器人在不同时间点的位置和方向信息
    for (int i = 0; i < path.poses.size(); ++i) {
        geometry_msgs::PoseStamped pose = path.poses[i];
        // 获取时间戳
        ros::Time timestamp = pose.header.stamp;
        // 获取机器人在该时刻的位姿信息
        geometry_msgs::Pose robot_pose = pose.pose;
        // 获取机器人在该时刻的位置信息（x,y,z坐标）
        double x = robot_pose.position.x;
        double y = robot_pose.position.y;
        double z = robot_pose.position.z;
        // 获取机器人在该时刻的方向信息（四元数表示）
        double qx = robot_pose.orientation.x;
        double qy = robot_pose.orientation.y;
        double qz = robot_pose.orientation.z;
        double qw = robot_pose.orientation.w;

        // 在这里对位置和方向信息进行处理
        // ...
    }
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "path_subscriber");
    ros::NodeHandle nh;

    // 创建一个订阅器，订阅名为 "/path_topic" 的话题
    ros::Subscriber sub = nh.subscribe<nav_msgs::Path>("/path_topic", 1000, pathCallback);

    // 进入循环，等待消息到来
    ros::spin();

    return 0;
}