导航时通过程序发布目标点，实现循环导航

使用action通信方式，客户端提交目标点，服务端订阅处理，在客户端实时发布反馈位姿信息，具体通信原理可参考官网的action通信介绍。

服务端：

/*
      需求：软件实现一系列目标点的发送
                  1.包含头文件；
                  2.节点初始化；
                  3.创建NodeHandle；
                  4.创建action服务端对象；
                  5.处理请求，产生反馈与响应
                  6.回旋函数spin().
*/

这部分重点主要是订阅目标位姿，并发布出去。

void CallBack(const move_base_msgs::MoveBaseGoalConstPtr &goal,Movebase_Server  *movebase_server)
{
      move_base_msgs::MoveBaseGoal  aim_goal;
      aim_goal.target_pose.header.frame_id=goal->target_pose.header.frame_id;
      aim_goal.target_pose.header.stamp=goal->target_pose.header.stamp;
      aim_goal.target_pose.pose.position.x=goal->target_pose.pose.position.x;
      aim_goal.target_pose.pose.position.y=goal->target_pose.pose.position.y;
      aim_goal.target_pose.pose.position.z=goal->target_pose.pose.orientation.w;
      ROS_INFO("提交的目标位姿为:x=%.2f,y=%.2f,w=%.2f",
                              aim_goal.target_pose.pose.position.x,
                              aim_goal.target_pose.pose.position.y,
                              aim_goal.target_pose.pose.orientation.w);
      move_base_msgs::MoveBaseFeedback  fb;
      fb.base_position=aim_goal.target_pose;
      movebase_server->publishFeedback(fb);
}

客户端：

/*
      需求：软件实现一系列目标点的发送
                  1.包含头文件；
                  2.节点初始化；
                  3.创建NodeHandle；
                  4.创建action客户端对象；
                  5.发送请求;
                        a.服务端与客户端建立连接；---回调函数
                        b.处理连续反馈；-----------------回调函数
                        c.处理最终响应。-----------------回调函数
                  6.回旋函数spin().
*/

//处理连续反馈

void feedback_cb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
{
      ROS_INFO("机器人当前位姿为:[x=%.2f,y=%.2f,w=%.2f]",
                              feedback->base_position.pose.position.x,
                              feedback->base_position.pose.position.y,
                              feedback->base_position.pose.orientation.w);
}

 初始化位姿，发送目标点，并在回调函数中进行处理。

void done_cb(const actionlib::SimpleClientGoalState &state, 
             const move_base_msgs::MoveBaseResultConstPtr &result);//状态反馈
void  active_cb();// 服务端与客户端建立连接
void feedback_cb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback);// 处理连续反馈

 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>  MoveBase_Client(nh,"move_base",true);
      // 等待服务端启动
      // MoveBase_Client.waitForServer();
      while(!MoveBase_Client.waitForServer(ros::Duration(5.0)))
      {
            ROS_INFO("Waiting for the  move_base action server!");
      }
      // 5.发送请求;
      // 定义坐标数据
      int size_c=8;
      double  airfloat_coord[size_c][2]={{0,0},{1,0},{2,0},{2,1},{2,2},{1,2},{0,2},{0,1}};

      move_base_msgs::MoveBaseGoal  goal;
      goal.target_pose.header.frame_id="map";          
      goal.target_pose.header.stamp=ros::Time::now();
      // 读取数组中的目标点位置数据
      for (int pose_x = 0; pose_x <=size_c; pose_x++)
      // for (int pose_x = 0; pose_x <=GoalNums; pose_x++)
      {
            for (int pose_y = 0; pose_y <2; pose_y++)
            // for (int pose_y = 0; pose_y <=GoalNums; pose_y++)
           {
                  goal.target_pose.pose.position.x=airfloat_coord[pose_x][0];
                  if (pose_y==1)
                  {
                        goal.target_pose.pose.position.y=airfloat_coord[pose_x][pose_y];
                  }
                  // goal.target_pose.pose.position.x=pose_x;
                  // goal.target_pose.pose.position.y=pose_y;
                  goal.target_pose.pose.orientation.w=1.0;
                  // goal.target_pose.pose.orientation.w=atan2(goal.target_pose.pose.position.y,goal.target_pose.pose.position.x) ;

                  /*
                        void sendGoal(const move_base_msgs::MoveBaseGoal &goal, 
                                    boost::function<void (const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result)> done_cb,
                                    boost::function<void ()> active_cb, 
                                    boost::function<void (const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)> feedback_cb)
                  */
                  ROS_INFO("发送目标!");
                  MoveBase_Client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
                  MoveBase_Client.waitForResult();
                  ros::Duration(1.0).sleep();
           }
      }

此处粘贴部分程序，聪明的小伙伴根据我写的应该可以写出来了，要是想要完整的代码，我可就要不厚道的收费呀(http://t.csdn.cn/UUpbx)。