move_baseでゴールについたことを検知する

目的

　ROSのmove_base(Navigation Stack)でゴールについたことを検出する。
　ゴールについたことを検出して、電源を切る、UIを切り替える、音声を発する、などの機能を持たせたいときに必要となる。

ゴールの検出

move_baseは、"/move_base/status"を常にpublishしている。
"/move_base/status"をsubscribeすれば良い。
"/move_base/status"は、GoalStatusArray型で定義されており、
最新の状況を確認するには、配列の先頭を取り出し、GoalStatus型で読み込む。
GoalStatusの値がどういう状態を示すかは、下記のROSのページを参照のこと。

参考資料：
http://docs.ros.org/jade/api/actionlib_msgs/html/msg/GoalStatus.html

ソースコード

下記のように、"/move_base/status"をsubscribeして、GoalStatusの値に応じて条件分岐する。
cmakeで"move_base_msgs actionlib"を含めてコンパイルする。

main.cpp

#include <ros/ros.h>

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

void navStatusCallBack(const actionlib_msgs::GoalStatusArray::ConstPtr &status)
{
    int status_id = 0;
    //uint8 PENDING         = 0  
    //uint8 ACTIVE          = 1 
    //uint8 PREEMPTED       = 2
    //uint8 SUCCEEDED       = 3
    //uint8 ABORTED         = 4
    //uint8 REJECTED        = 5
    //uint8 PREEMPTING      = 6
    //uint8 RECALLING       = 7
    //uint8 RECALLED        = 8
    //uint8 LOST            = 9

    if (!status->status_list.empty()){
    actionlib_msgs::GoalStatus goalStatus = status->status_list[0];
    status_id = goalStatus.status;
    }

    if(status_id==1){
    //移動中

    }

    if((status_id==3)||(status_id==0)){
    //ゴールに到達・もしくはゴールに到達して待機中。

    }

}

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

    ros::NodeHandle nh;
    ros::Subscriber switch_sub;

    ros::Subscriber move_base_status_sub;
    move_base_status_sub = nh.subscribe<actionlib_msgs::GoalStatusArray>("/move_base/status", 10, &navStatusCallBack);
    ros::spin();

    return 0;
}