ROS坐标导航的 C++ 编程实现

在使用 Navigation 导航系统中，不能每次都手动设置导航目标点，所以需要编写程序代码实现导航功能。所以需要导航接口，Navigation 有多个导航接口，ROS官方推荐使用 Action 接口实现导航功能。
通过C++实现导航的Action Client 节点。

C++操作步骤：

1. 编写节点代码
2. 设置编译规则
3. 编译节点文件
4. 运行节点文件

1. 编写节点代码

在 VSCode 中打开 nav_pkg/src 新建文件 nav_client.cpp 文件：

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

先加载3个头文件

第一个是 ros 系统头文件
第二个是 move_base_msgs 的 MoveBaseAction 消息包格式
第三个是actionlib的 SimpleActionClient 头文件

其中 move_base_msgs 的 MoveBaseAction 消息包格式：包括目标、结果
、消息反馈三种消息包类型。

Action 客户端的声明：

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

将前面一长串代码取了个别名“MoveBaseClient”，在生成Action客户端对象时，就可以直接使用该别名。

编写main主函数：

int main(int argc, char** argv)
{
	return 0；
}

初始化节点：

	ros::init(argc, argv, "nav_client");

生成Action客户端对象：

    MoveBaseClient ac("move_base", true);

直接使用别名指代前面一长串代码，ac为Action客户端对象的名字，在该对象构造函数中，第一个参数是要连接的 Action Server 的名字，在这里为move_base，第二个参数设为true，表示ac在与move_base的通讯过程中，自动阻塞等待结果，不用再写代码调用spin()函数。

在调用move_base导航服务前，需要确认move_base导航服务已经开启：

    while(!ac.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("Waiting for the move_base action server to come up");
    }

这里调用ac的WaitForServer()函数，来查询导航的服务状态，在参数中又调用了ros的Duration()函数，它的参数表示等待服务启动的时间，单位为秒。
所以该句代码意思为：等待5秒钟，若在此期间，move_base导航服务启动了，则立即停止等待，返回true跳出while循环，开始后面的操作；若等待5秒钟后，move_base导航服务仍未启动，则返回false，while循环会进入下一个5秒等待，如此循环，直到导航服务启动再继续。

在确认move_base导航服务启动后，就可以给move_base发送导航请求了。主要是将导航目标点的坐标值和目标姿态发送给move_base。
先定义一个导航消息包：

move_base_msgs::MoveBaseGoal goal;

类型为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;

先是导航目标点的坐标值，坐标系为map
并打上时间戳
坐标数值：为目标点在map坐标系中的数值
目标姿态：四元数，只给w赋值，其他xyz都为0

消息包赋值后调用Action客户端对象的sendGoal()函数，将消息包goal发送给move_base，之后便可以等待导航结果了：

ROS_INFO("Sending goal");
ac.sendGoal(goal);

ac.waitForResult();

waitForResult()函数会阻塞卡住，直到move_base返回导航结果，才继续往后执行。

得到结果后调用Action客户端对象的getState()函数，查询move_base返回的状态：

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

return 0;

若为SUCCEEDED，说明move_base导航顺利到达了目的地，输出结果任务成功；否则输出任务失败。

2. 设置编译规则

在VSCode中打开nav_pkg的CMakeLists.txt文件，在文件末尾添加：

add_executable(nav_client
 src/nav_client.cpp
)
add_dependencies(nav_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(nav_client
  ${catkin_LIBRARIES}
)

3. 编译节点文件

直接编译

4. 运行节点文件

在终端中运行下面3条指令：

roslaunch wpr_simulation wpb_stage_robocup.launch
roslaunch nav_pkg nav.launch
rosrun nav_pkg nav_client