完成rviz自己goal的插件后，来写一个订阅和发布到action的节点

 

#include
#include
#include
#include
#include
#include
#include
#include
#include "signal.h"

double pose_x_;
double pose_y_;
double pose_z_;
double pose_w_;

int count = 0;

    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    std::vector pose_list;

    

void callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{

    pose_x_ = msg->pose.position.x;
    pose_y_ = msg->pose.position.y;
    pose_z_ = msg->pose.orientation.z;
    pose_w_ = msg->pose.orientation.w;
    ROS_INFO("pose_x_ : [%f]",pose_x_);
    ROS_INFO("pose_y_ : [%f]",pose_y_);
    ROS_INFO("pose_z_ : [%f]",pose_z_);
    ROS_INFO("pose_w_ : [%f]",pose_w_);

 

  //设置我们要机器人走的几个点。

    geometry_msgs::Pose pose;

    point.x = pose_x_;
    point.y = pose_y_;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = pose_z_;
    quaternion.w = pose_w_;
    pose.position = point;
    pose.orientation = quaternion;
    pose_list.push_back(pose);

    count++;

}

bool flag = true;

void sigint(int signum){
    flag = false;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv,"sub_action");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("goal",1000,callback);
    actionlib::SimpleActionClient ac("move_base",true);
    
    signal(3, sigint);
    while(ros::ok() && flag){
        ros::spinOnce();        
    }
    

    
while(ros::ok()){
  ROS_INFO("Waiting for move_base action server...");
  //等待60秒以使操作服务器可用
  if(!ac.waitForServer(ros::Duration(30)))
  {
    ROS_INFO("Can't connected to move base server");
 
  }
  ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");

  //循环通过四个航点
for(int j=0; j < count ; j++)
 {
    //初始化航点目标
     move_base_msgs::MoveBaseGoal goal;

     //使用地图框定义目标姿势
     goal.target_pose.header.frame_id = "map";

     //将时间戳设置为“now”
     goal.target_pose.header.stamp = ros::Time::now();

     //将目标姿势设置为第i个航点
     goal.target_pose.pose = pose_list[j];

     //让机器人向目标移动
     //将目标姿势发送到MoveBaseAction服务器
     ac.sendGoal(goal);

    //等1分钟到达那里
    bool finished_within_time = ac.waitForResult();

    //如果我们没有及时赶到那里，就会中止目标
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
 }

}

}