//代碼說(shuō)明：接收循環(huán)次數(shù)和多個(gè)導(dǎo)航點(diǎn)，對(duì)數(shù)據(jù)進(jìn)行處理，循環(huán)發(fā)布導(dǎo)航點(diǎn)至move_base
//需要的頭文件
#include?
#include?se_msgs/MovebaseAction.h>
#include?
#include?
#include?
//定義一個(gè)vector用以接收多個(gè)導(dǎo)航點(diǎn)
std::vector?pose_list;
//聲明循環(huán)次數(shù)和多導(dǎo)航點(diǎn)的接收者
ros::Subscriber?vec_sub;
ros::Subscriber?tim_sub;
//聲明循環(huán)次數(shù)，導(dǎo)航目標(biāo)個(gè)數(shù)，循環(huán)需要的中間值
int?Cycle_timesGoals_numcount?=?0;
//接收循環(huán)次數(shù)
void?timcallback（const?std_msgs::Int8::ConstPtr&?msg）
{
	Cycle_times?=?msg->data.c_str（）;
}
//接收導(dǎo)航點(diǎn)數(shù)組，循環(huán)發(fā)布導(dǎo)航點(diǎn)到move_base
void?callback（const?geometry_msgs::PoseArray::ConstPtr&?pose_array）
{
	//取得導(dǎo)航點(diǎn)個(gè)數(shù)
	Goals_num?=?pose_array->poses.size（）;
	//存入多個(gè)導(dǎo)航點(diǎn)
	for（int?i?=0;i	{
		pose_list.push_back（pose_array->poses[i]）;
	}	
	actionlib::SimpleActionClientse_msgs::MovebaseAction>?cycle_client（“move_base“true）;
	ROS_INFO（“Waiting?for?move_base?action?server...“）;
	if（!cycle_client.waitForServer（ros::Duration（60）））
	{
		ROS_INFO（“Can‘t?connected?to?move?base?server“）;
	}
	ROS_INFO（“Connected?to?move?base?server“）;
	ROS_INFO（“Starting?navigation?“）;
	ROS_INFO（“cycle?times?is?%d“Cycle_times）;
	ROS_INFO（“cycle?goals?is?%d“Goals_num）;
	//多次循環(huán)
	for?（int?i?=0?;?i	{
		//循環(huán)一次所發(fā)布的多個(gè)導(dǎo)航點(diǎn)
		while（count?		{
			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_list[count];
			cycle_client.sendGoal（goal）;
			//60S內(nèi)如果不能達(dá)到目標(biāo)點(diǎn)則放棄該目標(biāo)點(diǎn)
			bool?finished_within_time?=?cycle_client.waitForResult（ros::Duration（60））;
			if（!finished_within_time）
			{
				cycle_client.cancelGoal（）;
				ROS_INFO（“Timed?out?achieving?goal“）;
			}
			else
			{
				//導(dǎo)航成功
				if（cycle_client.getState（）?==?actionlib::SimpleClientGoalState::SUCCEEDED）
				{
					ROS_INFO（“Goal?%d?succeeded!“count+1）;
				}
				//導(dǎo)航失敗
				else
				{
					ROS_INFO（“move?plan?is?error“）;
				}
		
			}
			count?+=?1;
	}
	count?=?0;
	}
	//導(dǎo)航結(jié)束，清空存放多個(gè)導(dǎo)航點(diǎn)的vector
	ROS_INFO（“The?Cycle?Goals?is?over“）;
	pose_list.clear（）;
}

int?main（int?argcchar**?argv）
{
	//初始化并設(shè)置節(jié)點(diǎn)名
	ros::init（argcargv“cycle_nav“）;
	//創(chuàng)建句柄
	ros::NodeHandle?n;
	ROS_INFO（“Cycle_nav?is?start...“）;
	//定義循環(huán)次數(shù)的接收者
	tim_sub?=?n.subscribe（“Cycle_times“5timcallback）;
	//定義多導(dǎo)航點(diǎn)的接受者
	vec_sub?=?n.subscribe（“Nav_PoseArray“5callback）;
	//循環(huán)等待回調(diào)函數(shù)
	ros::spin（）;
	return?0;
}