ros机器人巡逻程序，利用move_base的action机制循环发布4个目标点，同时从json文件中设置读取目标点进行发布。

前言：csdn上许多用move_base循环发送目标点的程序都是发送过去之后进行时间等待，根本就不判断是否到达目标点，这样就会存在很多问题，如果在你规定的时间内没达到目标点，那任务岂不是就失败了，显然这不是正确的程序。

0:本程序已经打包成功能包，大家可以直接下载百度网盘链接，然后将其中json目录下的goal.json文件中四个点的坐标按照自己的要求进行修改，同时修改move_base_goalsending.cpp第28行读取json的路径，再进行catkin_make编译,source后用rosrun move_base_send_goal move_base_goalsending命令运行即可。运行时需要先启动move_base。

move_base_send_goal 提取码：pvswhttps://pan.baidu.com/s/1jO3VFD13WvOBoqQiBrYlUA

1.本程序基于ros里的action机制进行编写，熟悉move_base源码的同学应该知道，move_base实际就是一个actionlib，有关action的机制大家可以参考赵虚左老师的课程文章，链接如下。

action机制原理http://www.autolabor.com.cn/book/ROSTutorials/di-7-zhang-ji-qi-ren-dao-822a28-fang-771f29/1001-actiontong-xin/1011.html2.move_base实际就是一个类型为<move_base_msgs::MoveBaseAction>的actionlib::SimpleActionClient，有关actionlib::SimpleActionClient类下的一些功能函数我们可以参考这篇文章。

actionlib中的函数作用https://blog.csdn.net/qq_23670601/article/details/84936804在发送循环目标点，并判断目标点是否到达，我们主要用到action机制其中的两个函数，一个是sendgoal()函数，用于发送导航目标点。另一个是getstate()函数，用于获取当前action的状态（即当前目标是否到达，是否被抢占等等）。

void sendGoal (const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(),
 SimpleActiveCallback active_cb=SimpleActiveCallback(), 
SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())；
 发送一个目标值到动作服务器
#例子:ac.sendGoal(grigoal);

SimpleClientGoalState 	getState ()；
获取此目标的状态信息
例子：
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You have reached the goal!");
 
SimpleClientGoalState选项:
PENDING 	
ACTIVE 	
RECALLED 	
REJECTED 	
PREEMPTED 	
ABORTED 	
SUCCEEDED 	
LOST

3.以下是程序源码，先定义头文件，再定义源文件。代码解释我已经写在源码当中，这里就不过多叙述了。

move_base_goalsending.h

/**
 * @file goalsending.h
 * @author sifan
 * @brief  循环发布目标点程序，从json文件读取目标点进行发布,从move_base反馈接受机器人当前位姿
 * @version 0.1
 * @date 2022-08-26
 *
 * @copyright Copyright (c) 2022
 *
 */
#ifndef GOAL_SENDING_H
#define GOAL_SENDING_H

#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/PoseStamped.h>
#include <jsoncpp/json/json.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <ros/ros.h>
#include <fstream>
//给MoveBaseAction定义一个别名，方便创建对象
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;

class GoalSending {
 private:
  // ros
  ros::NodeHandle nh_;
  ros::Publisher goal_pub_;
  ros::Timer timer_;
  geometry_msgs::PoseStamped target_pose_;
  move_base_msgs::MoveBaseGoal goal_;
  //创建MoveBaseAction对象
  Client ac_;

  bool initialized_;
  int count = 0;
  // 用于读取json中的数据
  Json::Reader reader_;
  Json::Value root_;
  std::ifstream in_;

  //存放目标点的数组
  double goal_point_[4][2];
  /**
   * @brief 发布目标点
   */
  void goalPointPub(const ros::TimerEvent& event);
  /**
   * @brief 打开Json文件，读取目标点,赋值给goal_point_数组
   */
  void openFile();
  //判断是否接收到目标点
  void activeCb();
  void doneCb(const actionlib::SimpleClientGoalState& state,
              const move_base_msgs::MoveBaseResultConstPtr& result);

 public:
  GoalSending();
  ~GoalSending();
};

#endif  // GOAL_SENDING_H

move_base_goalsending.cpp

/**
 * @file goalsengding.cpp
 * @author sifan
 * @brief
 * 循环发布目标点程序，从json文件读取目标点进行发布,从move_base反馈状态判断机器人是否达到目标点，四个点的目标循环，如需增加点，则修改goal_point的维度大小。
 *         或者将存放目标点的数组改为vector储存的形式，可以不限制点数量。
 * @version 0.1
 * @date 2022-08-26
 *
 * @copyright Copyright (c) 2022
 *
 */
#include "move_base_goalsending.h"
#include <iostream>

GoalSending::GoalSending() : ac_("move_base", true), initialized_(false) {
  //从json文件中读取目标点
  this->openFile();
  //构建定时器，定时判断是否达到目标点，以发送下一个目标
  timer_ =
      nh_.createTimer(ros::Duration(0.3), &GoalSending::goalPointPub, this);

  ROS_INFO("Starting success");
}

void GoalSending::openFile() {
  //使用时需要将该路径替换为自己json文件对应的路径
  in_.open(
      "/home/robint01/robintros_ws/src/RobintROS/move_base_send_goal/json/"
      "goal.json",
      std::ios::binary);
  if (!in_.is_open()) {
    std::cout << "Error opening file
";
    return;
  }
  if (reader_.parse(in_, root_)) {
    for (int i = 0; i < 4; i++) {
      goal_point_[i][0] = root_["goalname"][i]["location"][0].asDouble();
      goal_point_[i][1] = root_["goalname"][i]["location"][1].asDouble();
    }
  }
}
void GoalSending::doneCb(const actionlib::SimpleClientGoalState& state,
                         const move_base_msgs::MoveBaseResultConstPtr& result) {
  if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
    ROS_INFO("SUCCEEDED");
  }
}
void GoalSending::activeCb() {
  ROS_INFO("Goal Received");
}

void GoalSending::goalPointPub(const ros::TimerEvent& event) {
  //发送初始点位
  if (!initialized_) {
    target_pose_.header.seq = 1;
    target_pose_.header.frame_id = "map";
    target_pose_.header.stamp = ros::Time::now();
    target_pose_.pose.position.x = goal_point_[0][0];
    target_pose_.pose.position.y = goal_point_[0][1];
    target_pose_.pose.orientation.z = 0.01;
    target_pose_.pose.orientation.w = 0.01;
    goal_.target_pose = target_pose_;
    initialized_ = true;
    if (!ac_.waitForServer(ros::Duration(60))) {
      ROS_INFO("Can't connected to move base server");
    }
    ac_.sendGoal(goal_, boost::bind(&GoalSending::doneCb, this, _1, _2),
                 boost::bind(&GoalSending::activeCb, this),
                 Client::SimpleFeedbackCallback());
  }
  //判断是否存在抢占，如果有抢占，则立即重新发布目标（即不允许抢占情况出现，如想可以发生抢占,注释掉这个if语句）
  if (ac_.getState() == actionlib::SimpleClientGoalState::PREEMPTED) {
    ac_.sendGoal(goal_, boost::bind(&GoalSending::doneCb, this, _1, _2),
                 boost::bind(&GoalSending::activeCb, this),
                 Client::SimpleFeedbackCallback());
    ROS_WARN("No preemption");
  }
  //判断是否到达目标点，如果成功，则发布下一个点
  if (ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
    count++;
    target_pose_.header.seq = count + 1;
    target_pose_.header.frame_id = "map";
    target_pose_.header.stamp = ros::Time::now();
    target_pose_.pose.position.x = goal_point_[count][0];
    target_pose_.pose.position.y = goal_point_[count][1];
    target_pose_.pose.orientation.z = 0.01;
    target_pose_.pose.orientation.w = 0.01;
    goal_.target_pose = target_pose_;
    if (!ac_.waitForServer(ros::Duration(60))) {
      ROS_INFO("Can't connected to move base server");
    }
    ac_.sendGoal(goal_, boost::bind(&GoalSending::doneCb, this, _1, _2),
                 boost::bind(&GoalSending::activeCb, this),
                 Client::SimpleFeedbackCallback());
  }
  //如果已经到达第四个点，则初始化，回到第一个点，循环运动
  if (count == 4) {
    initialized_ = false;
    count = 0;
  }
}

GoalSending::~GoalSending() {
  in_.close();
}
int main(int argc, char** argv) {
  ros::init(argc, argv, "goalsending");
  GoalSending gs;
  ros::spin();
  return 0;
}