获取RVIZ中的publish point的目标点进行多点导航功能C++代码

以下是通过C++代码获取RVIZ中的publish point的目标点进行多点导航的示例代码：

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

std::vector<geometry_msgs::PoseStamped> goals;

void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
    ROS_INFO("Received a new goal position");
    goals.push_back(*msg);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "multi_goal_navigation");
    ros::NodeHandle nh;

    // Subscribe to the goal topic
    ros::Subscriber sub = nh.subscribe<geometry_msgs::PoseStamped>("move_base_simple/goal", 10, goalCallback);

    // Wait for the first goal to be received
    while (ros::ok() && goals.empty()) {
        ROS_INFO("Waiting for the first goal to be received...");
        ros::spinOnce();
        ros::Duration(1.0).sleep();
    }

    // Start navigation to the first goal
    ROS_INFO("Starting navigation to the first goal...");
    // Code to call the navigation stack to move to the first goal

    // Wait for the next goal to be received and continue navigation
    for (size_t i = 1; i < goals.size(); ++i) {
        ROS_INFO("Waiting for the next goal to be received...");
        while (ros::ok() && goals.size() <= i) {
            ros::spinOnce();
            ros::Duration(1.0).sleep();
        }

        ROS_INFO("Starting navigation to the next goal...");
        // Code to call the navigation stack to move to the next goal
    }

    ROS_INFO("All goals have been reached, stopping navigation...");
    return 0;
}

该代码订阅了"move_base_simple/goal"话题，在收到新的目标点时将其存储在一个vector中。在等待第一个目标点时，程序会不断调用ros::spinOnce()和ros::Duration()函数，以确保ROS系统能够处理所有的回调函数。在收到第一个目标点后，程序会开始导航到该目标点，并在等待下一个目标点时重复这个过程。当所有的目标点都被导航到时，程序会停止导航并退出。