ros多点导航实现C++代码

下面是一个简单的示例C++代码，用于在ROS中实现多点导航：

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv){
  ros::init(argc, argv, "multi_point_navigation");

  // 创建移动基地客户端
  MoveBaseClient ac("move_base", true);

  // 等待移动基地服务器启动
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("等待 move_base 服务器启动...");
  }

  // 定义多个目标点
  move_base_msgs::MoveBaseGoal goals[3];

  // 第一个目标点
  goals[0].target_pose.header.frame_id = "map";
  goals[0].target_pose.header.stamp = ros::Time::now();
  goals[0].target_pose.pose.position.x = 1.0;
  goals[0].target_pose.pose.position.y = 2.0;
  goals[0].target_pose.pose.orientation.w = 1.0;

  // 第二个目标点
  goals[1].target_pose.header.frame_id = "map";
  goals[1].target_pose.header.stamp = ros::Time::now();
  goals[1].target_pose.pose.position.x = 3.0;
  goals[1].target_pose.pose.position.y = 4.0;
  goals[1].target_pose.pose.orientation.w = 1.0;

  // 第三个目标点
  goals[2].target_pose.header.frame_id = "map";
  goals[2].target_pose.header.stamp = ros::Time::now();
  goals[2].target_pose.pose.position.x = 5.0;
  goals[2].target_pose.pose.position.y = 6.0;
  goals[2].target_pose.pose.orientation.w = 1.0;

  // 发送目标点
  for(int i = 0; i < 3; i++){
    ROS_INFO("移动到目标点 %d...", i+1);
    ac.sendGoal(goals[i]);

    // 等待机器人到达目标点
    ac.waitForResult();

    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
      ROS_INFO("机器人已到达目标点 %d!", i+1);
    else
      ROS_INFO("机器人无法到达目标点 %d！", i+1);
  }

  return 0;
}

该代码通过创建一个移动基地客户端来向移动基地服务器发送目标点，并等待机器人到达目标点。在该示例中，我们定义了三个目标点，但您可以根据需要添加或删除目标点。请注意，您需要在ROS中正确配置move_base包，以便正确运行该代码。