ros move_base中多个导航点的c++实现

ros move_base提供了actionlib的服务，用户可以通过该服务来设置一系列导航位置点，该例子提供了c++的一个实现，通过使用相应的回调函数，用户还可以随时取消不需要的导航点。

#include 
#include 
#include se_msgs/MovebaseAction.h>
#include “tf/tf.h“
#include 
#include 
typedef actionlib::SimpleActionClientse_msgs::MovebaseAction> MovebaseClient;
class MyNode
{
public:

	MovebaseClient* ac_;
	void init（） {
		//MovebaseClient ac（“move_base“ true）;
		ac_ = new MovebaseClient（“move_base“ true）;
		//wait for the action server to come up
		while （!ac_->waitForServer（ros::Duration（5.0））） {
			ROS_INFO（“Waiting for the move_base action server to come up“）;
		}
	}

	bool doStuff（const double& x_set const double& y_set const double& z_set const double& x const double& y const double& z const double& w）
	{
		move_base_msgs::MovebaseGoal goal;
		if （x_set != last_x_set || y_set != last_y_set）
		{
			//we‘ll send a goal to the robot to move 1 meter forward
			goal.target_pose.header.frame_id = “map“;
			goal.target_pose.header.stamp = ros::Time::now（）;

			goal.target_pose.pose.position.x = x_set;
			goal.target_pose.pose.position.y = y_set;
			goal.target_pose.pose.position.z = 0;

			//          tf::Quaternion q;
			//          q.setRotation（tf::Vector3（0 0 1） th_set）;
			goal.target_pose.pose.orientation.x = x;
			goal.target_pose.pose.orientation.y = y;
			goal.target_pose.pose.orientation.z = z;
			goal.target_pose.pose.orientation.w = w;

			ROS_INFO（“Sending goal“）;
			// ac.sendGoal（goal）;
			ac_->sendGoal（goal
				boost::bind（&MyNode::doneCb this _1 _2）
				MovebaseClient::SimpleActiveCallback（）
				MovebaseClient::SimpleFeedbackCallback（））;
			success_ = false;
		}
		last_x_set = x_set; last_y_set = y_set;
		return success_;
	}