获取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
    size_t i = 0;
    while (ros::ok()) {
        ROS_INFO("Starting navigation to goal %zu...", i+1);
        // Code to call the navigation stack to move to the current goal
        // If the navigation was successful, move to the next goal
        ++i;
        if (i >= goals.size()) {
            ROS_INFO("All goals have been reached, restarting navigation cycle...");
            i = 0;
        }
        // Wait for the next goal to be received
        while (ros::ok() && goals.size() <= i) {
            ROS_INFO("Waiting for the next goal to be received...");
            ros::spinOnce();
            ros::Duration(1.0).sleep();
        }
    }

    ROS_INFO("Navigation stopped.");
    return 0;
}

该代码与前面的示例代码类似，不同之处在于当所有的目标点都被导航到后，程序会重新从第一个目标点开始导航，并一直循环执行下去，直到程序被停止。在程序中，使用一个循环变量i来遍历所有的目标点，当i超过目标点的数量时，将i重置为0，以实现循环导航的功能。