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 提取码：pvsw
https://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实际就是一个类型为
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
#include
#include
#include
#include
#include
//给MoveBaseAction定义一个别名，方便创建对象
typedef actionlib::SimpleActionClient 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
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\n";
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;
}