ROSNOTE : 用代码控制机器人移动到目标点

(1) 代码的开始部分，先include了三个头文件，第一个是ros的系统头文件，第二个是导航目标结构体move_base_msgs::MoveBaseGoal的定义文件，第三个是actionlib::SimpleActionClient的定义文件。

(2) ROS节点的主体函数是int main(int argc, char** argv)，其参数定义和其他C++程序一样。

(3) main函数里，首先调用ros::init(argc, argv, "demo_simple_goal");进行该节点的初始化操作，函数的第三个参数是节点名称。

(4) 接下来声明一个MoveBaseClient对象ac，用来调用和主管监控导航功能的服务。

(5) 在请求导航服务前，需要确认导航服务已经开启，所以这里调用ac.waitForServer()函数来查询导航服务的状态。ros::Duration()是睡眠函数，参数的单位为秒，表示睡眠一段时间，这段时间若被某个信号打断（在这个例程里，这个信号就是导航服务已经启动的信号），则中断睡眠。所以ac.waitForServer(ros::Duration(5.0));的意思就是：休眠5秒，若期间导航服务启动了，则中断睡眠，开始后面的操作。用while循环来嵌套，可以让程序在休眠5秒后，若导航服务未启动，则继续进入下一个5秒睡眠，直到导航服务启动才中断睡眠。

(6) 确认导航服务启动后，我们声明一个move_base_msgs::MoveBaseGoal类型结构体对象goal，用来传递我们要导航去的目标信息。
goal.target_pose.header.frame_id表示这个目标位置的坐标是基于哪个坐标系，例程里赋值“map”表示这是一个基于全局地图的导航目标位置。
goal.target_pose.header.stamp赋值当前时间戳。
goal.target_pose.pose.position.x 赋值-3.0，表示本次导航的目的地是以地图坐标系为基础，向X轴反方向移动3.0米。
goal.target_pose.pose.position的y赋值2.0，表示本次导航的目的地是以地图坐标系为基础，向Y轴正方向移动2.0米。
goal.target_pose.pose.position的z未赋值，则默认是0；
goal.target_pose.pose.orientation.w 赋值1.0，则导航的目标姿态是机器人面朝X轴的正方向（正前方）。

(7) ac.sendGoal(goal);将导航目标信息传递给导航服务的客户端ac，由ac来监控后面的导航过程。

(8) ac.waitForResult();等待MoveBase的导航结果，这个函数会保持阻塞，就是卡在这，直到整个导航过程结束，或者导航过程被其他原因中断。

(9) ac.waitForResult()阻塞结束后，调用ac.getState()获取导航服务的结果，如果是“SUCCEEDED”说明导航顺利到达目的地，输出结果“Mission complete!”；若不是这个结果，输出结果“Mission failed ...”。

 

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "demo_simple_goal");

  //tell the action client that we want to spin a thread by default
  //声明一个MoveBaseClient对象ac，用来调用和主管监控导航功能的服务。
  MoveBaseClient ac("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");
  }

  //用来传递我们导航去的目标信息
  move_base_msgs::MoveBaseGoal goal;

  goal.target_pose.header.frame_id = "map";
  goal.target_pose.header.stamp = ros::Time::now();

  goal.target_pose.pose.position.x = -3.0;
  goal.target_pose.pose.position.y = 2.0;
  goal.target_pose.pose.orientation.w = 1.0;

  ROS_INFO("Sending goal");
  //将导航目标信息传递给导航服务的客户端ac，由ac来监控后面的导航过程。
  ac.sendGoal(goal);

  ac.waitForResult();

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Mission complete!");
  else
    ROS_INFO("Mission failed ...");

  return 0;
}