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;
}