机器人导航：move_base的actionlib机制

通过actionlib客户端发布目标点 让actionlib 服务器响应。
在actionlin客户端

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "nav_move_base");
    ros::NodeHandle node;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);//订阅move_base操作服务器
    //设置目标点
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    geometry_msgs::Pose pose;
    point.x = 17;
    point.y = 28;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.012;
    quaternion.w = 0.999;
    pose.position = point;
    pose.orientation = quaternion;
    ROS_INFO("waiting movebase server.");
    if(!ac.waitForServer(ros::Duration(30)))//等待操作服务器可用
    {
        ROS_INFO("not connect movebase server.");
        return 1;
    }
    ROS_INFO("connected movebase server success.");
    ROS_INFO("start to navigation.");
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = pose;
    ac.sendGoal(goal);//将目标位姿发送到MoveBaseAction服务器
    bool finished_within_time = ac.waitForResult(ros::Duration(1000));//要求机器人在1000s时间内到达目标点
    if(!finished_within_time)//没有及时到达，放弃目标
    {
        ac.cancelGoal();
        ROS_INFO("time out for arrival goal.");
    }
    else
    {
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("succeess to arrival goal!");
        }
        else
        {
            ROS_INFO("other reason to failed to arrival goal.");
        }
    }
    ros::spin();
    ROS_INFO("movebase kill...");
    return 0;
}

在actionlib的服务器端，梳理了navigation的movebase中的使用。

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/server/simple_action_server.h>
#include <cmath>

class MoveBase
{
private:
    /* data */
public:
    MoveBase(/* args */);
    ~MoveBase();
    void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal);
    bool isQuaternionValid(const geometry_msgs::Quaternion& q);
    void resetState();
public:
    typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
    MoveBaseActionServer* as_;
};
void MoveBase::resetState(){}
bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){}

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)
{

    //以goal消息指针作为输入参数 检查目标点位姿是否合法
    if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation))
    {
      as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
      return;
    }
    /* code */
    ros::NodeHandle n;
    while (n.ok())
    {
        /* code */    
        if(as_->isPreemptRequested())//查看action服务器的状态机，如果触发抢占，则相应做出处理，并更新as_的状态机
        {
            if(as_->isNewGoalAvailable())//因为收到新的目标而触发抢占，接收新目标点并重新开始新的规划控制
            {
                move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
                if(!isQuaternionValid(new_goal.target_pose.pose.orientation))
                {
                    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
                    return;
                }
            }
            else//其它抢占行为，则取消aciton任务，挂起退出
            {
                resetState();
                ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
                as_->setPreempted();
                return;
            }
        }

    }
    /* code */
    //在while循环中，以ros::ok作为终止业务循环的条件，如果因为该条件而终止业务，意味着move_base节点被杀死，需要更新Action服务器对象 as_状态。
    as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed");

}
MoveBase::MoveBase(/* args */):as_(NULL)
{
    //构建Action服务器对象as_并开启服务。构建对象as_的时候提供回调函数executeCb，每当有新的Action请求的时候， 都会调用这一回调函数，进行规划控制。
    as_= new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
    as_->start();
}

MoveBase::~MoveBase()
{
    if(as_ != NULL)
        delete as_;
}

通过以上梳理，熟悉了actionlib在MoveBase中的基本使用，以及通过客户端发布目标点在回调函数中让服务器响应的部分业务逻辑。