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, "loop_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;

  // 发送目标点
  while(ros::ok()){
    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;
}

该代码与前面的示例非常相似，但是在最后添加了一个while循环，以便机器人可以无限循环导航。在该示例中，我们定义了三个目标点，并使用一个for循环来遍历这些点。在机器人到达最后一个点之后，它将重新从第一个目标点开始导航。请注意，您需要在ROS中正确配置move_base包，以便正确运行该代码。