连载文章，长期更新，欢迎关注：
写在前面
第1章-ROS入门必备知识
1.1 ROS简介
1.2 ROS开发环境搭建
1.3 ROS系统架构
1.4 ROS调试工具
1.5 ROS节点通信
1.6 ROS其他重要概念
1.7 ROS2.0展望
第2章-C++编程范式
第3章-OpenCV图像处理
第4章-机器人传感器
第5章-机器人主机
第6章-机器人底盘
第7章-SLAM中的数学基础
第8章-激光SLAM系统
第9章-视觉SLAM系统
第10章-其他SLAM系统
第11章-自主导航中的数学基础
第12章-典型自主导航系统
第13章-机器人SLAM导航综合实战
前面已经学习了大量ROS的基础知识，到这里终于可以开始编写ROS代码了，并在实战中体验和理解ROS的真正面容。ROS代码的编写围绕节点通信过程中的消息机制和消息类型两个核心点展开，因此先详细阐述话题（topic）、服务（service）和动作（action）三种消息机制的原理，然后介绍这三种消息机制中使用的消息类型，最后利用C++编写分别使用这三种消息机制通信的代码实例。
话题通信方式是单向异步的，发布者只负责将消息发布到话题，订阅者只从话题订阅消息，发布者与订阅者之间并不需要事先确定对方的身份，话题充当消息存储容器的角色，这种机制很好地实现了发布者与订阅者程序之间的解耦。由于话题通信方式是单向的，即发布者并不能确定订阅者是否按时接收到消息，所以这种机制也是异步的。话题通信一般用在实时性要求不高的场景中，比如传感器广播其采集的数据。如图1-10所示，是一个通过话题消息机制传递hello消息内容的实例。
图1-10 话题消息机制的过程
服务通信方式是双向同步的，服务客户端向服务提供端发送请求，服务提供端在收到请求后立马进行处理并返回响应信息。如图1-11所示，是一个通过服务消息机制计算两个数之和的实例。服务通信一般用在实时性要求比较高且使用频次低的场景下，比如获取全局静态地图。
图1-11 服务消息机制的过程
动作通信方式是双向异步的，动作客户端向动作提供端发送目标后，要达到目标需要一个过程，在执行目标的过程中实时地反馈消息，并且在目标完成后返回结果。动作通信用在一个过程性的任务执行场景下，比如导航任务。如图1-12所示，是一个通过动作消息机制实现倒计时任务的实例。
图1-12 动作消息机制的过程
进一步探究消息机制的底层实现，能够帮助大家更深入地理解ROS的性能特点。ROS的消息机制基于XMLRPC（XML Remote Procedure Call，XML远程过程调用）协议，这是一种采用XML编码格式，传输方式既不保持连接状态、也不检测连接状态的请求/响应式的HTTP协议。ROS的主节点master采用XMLRPC协议对节点的注册和连接进行管理，一旦两个节点建立了连接，节点之间就可以利用TCP/UDP协议传输消息数据了。如图1-13所示，是XMLRPC通信模型。在话题通信中，节点需要借助XMLRPC完成注册和连接，然后订阅者发起订阅之后，发布者就开始持续发布消息；在服务通信中，节点借助XMLRPC完成注册，而不需要建立连接就可以直接发起请求，响应完成后就自动断开；在动作通信中，节点借助XMLRPC完成注册，跟服务通信类似也不需要建立连接就可以直接发起目标，只是在响应的基础上多了一个反馈过程，完成后就自动断开。
图1-13 XMLRPC通信模型
了解ROS消息机制的原理后，接下来讨论一下ROS中的消息类型。其实消息类型就是一种数据结构，最底层的数据结构还是C++/Python的基本数据类型，只是ROS利用这些基本数据类型做了自己的封装。ROS中的消息类型分两种：一种是ROS定义的标准消息类型，另一种是用户利用标准消息类型自己封装的非标准消息类型。可以说用户自定义的非标准消息类型是对标准消息类型的有效补充。不管是标准的消息类型还是自定义的消息类型，都需要在功能包中进行封装，因此使用消息类型的时候需要使用功能包名和子类型名同时进行标识。ROS标准消息类型主要封装在std_msgs、sensor_msgs、geometry_msgs、nav_msgs、actionlib_msgs这些功能包中，并且这些消息类型大部分用在话题通信中，如表1-5所示。
表1-5 ROS标准消息类型
| 封装 功能包 | std_msgs | sensor_msgs | geometry_msgs | nav_msgs | actionlib_msgs |
| 话题 消息 类型 | Bool Byte ByteMultiArray Char ColorRGBA Duration Empty Float32 Float32MultiArray Float64 Float64MultiArray Header Int16 Int16MultiArray Int32 Int32MultiArray Int64 Int64MultiArray Int8 Int8MultiArray MultiArrayDimension MultiArrayLayout String Time UInt16 UInt16MultiArray UInt32 UInt32MultiArray UInt64 UInt64MultiArray UInt8 UInt8MultiArray | BatteryState CameraInfo ChannelFloat32 CompressedImage FluidPressure Illuminance Image Imu JointState Joy JoyFeedback JoyFeedbackArray LaserEcho LaserScan MagneticField MultiDOFJointState MultiEchoLaserScan NavSatFix NavSatStatus PointCloud PointCloud2 PointField Range RegionOfInterest RelativeHumidity Temperature TimeReference | Accel AccelStamped AccelWithCovariance AccelWithCovarianceStamped Inertia InertiaStamped Point Point32 PointStamped Polygon PolygonStamped Pose Pose2D PoseArray PoseStamped PoseWithCovariance PoseWithCovarianceStamped Quaternion QuaternionStamped Transform TransformStamped Twist TwistStamped TwistWithCovariance TwistWithCovarianceStamped Vector3 Vector3Stamped Wrench WrenchStamped | GridCells MapMetaData OccupancyGrid Odometry Path | GoalID GoalStatus GoalStatusArray |
| 服务 消息 类型 | SetCameraInfo | GetMap GetPlan SetMap | |||
| 动作 消息 类型 | GetMap |
如果想要了解ROS标准消息类型的详细定义以及具体用法，可以查阅以下一些ROS的官方wiki页面：
现在我们就来编写真正意义上使用ROS进行节点间通信的程序。由于之前已经建好了catkin_ws这样一个工作空间，以后开发的功能包都将放在这个工作空间。这里给新建的功能包取名为topic_example，在这个功能包中分别编写publish_node.cpp和subscribe_node.cpp两个节点程序，发布节点publish_node向话题chatter发布std_msgs::String类型的消息，订阅节点subscribe_node从话题chatter订阅std_msgs::String类型的消息，这里消息传递的具体内容是一句问候语hello，具体过程如图1-10所示。
1.创建功能包
在~/catkin_ws/src/目录下新建功能包topic_example，并在创建时显式地指明依赖roscpp和std_msgs。打开命令行终端，输入如下命令。
cd ~/catkin_ws/src/
#创建功能包topic_example时，显式地指明依赖roscpp和std_msgs，
#依赖会被默认写到功能包的CMakeLists.txt和package.xml中
catkin_create_pkg topic_example roscpp std_msgs2.编写功能包源码
功能包中需要编写两个独立可执行的节点，一个节点用来发布消息，另一个节点用来订阅消息，所以需要在新建的功能包topic_example/src/目录下新建两个文件publish_node.cpp和subscribe_node.cpp。首先来看看发布节点，发布节点publish_node.cpp代码内容见代码清单1-1。
代码清单1-1 发布节点publish_node.cpp
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 #include <sstream>
5
6 int main(int argc, char **argv)
7 {
8 ros::init(argc, argv, "publish_node");
9 ros::NodeHandle nh;
10
11 ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
12 ros::Rate loop_rate(10);
13 int count = 0;
14
15 while (ros::ok())
16 {
17 std_msgs::String msg;
18
19 std::stringstream ss;
20 ss << "hello " << count;
21 msg.data = ss.str();
22 ROS_INFO("%s", msg.data.c_str());
23
24 chatter_pub.publish(msg);
25
26 ros::spinOnce();
27 loop_rate.sleep();
28 ++count;
29 }
30
31 return 0;
32 }第1行，是必须包含的头文件，这是ROS提供的C++客户端库。
第2行，是包含ROS提供的标准消息类型std_msgs::String的头文件，这里使用标准消息类型来发布话题通信的消息。
第8行，这句是初始化ros节点并指明节点的名称，这里给节点取名为publish_node，一旦程序运行后就可以在ros的计算图中被注册为publish_node名称标识的节点。
第9行，这句是声明一个ros节点的句柄，初始化ros节点必须的。
第11行，这句话告诉ros节点管理器我们将会在chatter这个话题上发布std_msgs::String类型的消息。这里的参数1000是表示发布序列的大小，如果消息发布的太快，缓冲区中的消息大于1000个的话就会开始丢弃先前发布的消息。
第12行，这句话是用来指定自循环的频率，这里的参数10 表示10Hz频率，需要配合该对象的sleep()方法来使用。
第15行，调用roscpp库时会默认安装SIGINT句柄，这句话就是用来处理由Ctrl+C键盘操作、该节点被另一同名节点踢出ROS网络、ros::shutdown()被程序在某个地方调用、所有ros::NodeHandle句柄都被销毁等触发而使ros::ok()返回false值的情况。
第17行，定义了一个std_msgs::String消息类型的对象，该对象有一个数据成员data用于存放我们即将发布的数据。要发布出去的数据将被填充到这个对象的data成员中。
第24行，利用定义好的发布器对象将消息数据发布出去，这一句执行后，ROS网络中的其他节点便可以收到此消息中的数据。
第26行，这句是让回调函数有机会被执行的声明，这个程序里面并没有回调函数，所以这一句可以不要，这里只是为了程序的完整规范性才放上来的。
第27行，前面讲过，这一句是通过休眠来控制自循环的频率的。
其实将发布节点的代码稍作修改，就能得到订阅节点。接着来看看订阅节点，订阅节点subscribe_node.cpp代码内容见代码清单1-2。
代码清单1-2 订阅节点subscribe_node.cpp
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 void chatterCallback(const std_msgs::String::ConstPtr& msg)
5 {
6 ROS_INFO("I heard: [%s]",msg->data.c_str());
7 }
8
9 int main(int argc, char **argv)
10 {
11 ros::init(argc, argv, "subscribe_node");
12 ros::NodeHandle nh;
13
14 ros::Subscriber chatter_sub = nh.subscribe("chatter", 1000,chatterCallback);
15
16 ros::spin();
17
18 return 0;
19 }在发布节点中已经解释过的类似代码，就不再赘述了。这里重点解释一下前面没遇到过的代码。
第4～7行，这是一个回调函数，当有消息到达chatter话题时会自动被调用一次，这个回调函数里面就是一句话，用来打印从话题中订阅的消息数据。
第14行，这句话告诉ros节点管理器我们将会从chatter这个话题中订阅消息，当有消息到达时会自动调用这里指定的chatterCallback回调函数。这里的参数1000是表示订阅序列的大小，如果消息处理的速度不够快，缓冲区中的消息大于1000个的话就会开始丢弃先前接收的消息。
第16行，这一句话让程序进入自循环的挂起状态，从而让程序以最大效率接收消息并调用回调函数。如果没有消息到达，这句话不会占用很多CPU资源，所以这句话可以放心使用。一旦ros::ok()被触发而返回false，ros::spin()的挂起状态将停止并自动跳出。简单点说，程序执行到这一句，就在这里不断自循环，与此同时检查是否有消息到达并决定是否调用回调函数。
3.配置和编译功能包
创建功能包topic_example时，显式地指明了依赖roscpp和std_msgs，该依赖已经被默认写到功能包的CMakeLists.txt和package.xml中，所以只需要在CMakeLists.txt文件的末尾行加入以下几句用于声明可执行文件就可以了，见代码清单1-3。add_executable用于创建可执行文件，也就是将源码编译成可执行文件；target_link_libraries用于连接可执行文件运行时需要的依赖库。
代码清单1-3 在CMakeLists.txt添加编译项
1 add_executable(publish_node src/publish_node.cpp)
2 target_link_libraries(publish_node ${catkin_LIBRARIES})
3
4 add_executable(subscribe_node src/subscribe_node.cpp)
5 target_link_libraries(subscribe_node ${catkin_LIBRARIES})修改好CMakeLists.txt文件末尾的编译配置后，就可以编译我们的功能包了，编译命令如下。其中catkin_make命令后面的参数DCATKIN_WHITELIST_PACKAGES指定需要编译的功能包，直接使用cakin_make的话就是编译工作空间中的所有功能包。如果新建的功能包加入工作空间后，使用catkin_make执行编译无响应，需要把catkin_ws/build/CATKIN_IGNORE删除或者直接删除catkin_ws/build后重新执行编译。
cd ~/catkin_ws/
catkin_make -DCATKIN_WHITELIST_PACKAGES="topic_example"4.启动功能包中的节点
首先，需要用roscore命令来启动ROS节点管理器，ROS节点管理器是所有节点运行的基础。打开命令行终端，输入如下命令。
roscore然后，就可以用rosrun命令来启动功能包topic_example中的节点publish_node，发布我们的消息了。打开另外一个命令行终端，输入如下命令。
rosrun topic_example publish_node启动完publish_node节点后，可以在终端中看到打印信息不断输出，如图1-14所示。这就说明发布节点已经正常启动，并不断向chatter话题发布消息数据了。
图1-14 启动publish_node节点发布数据
最后，用rosrun命令来启动功能包topic_example中的节点subscribe_node，订阅上面发布出来的消息了。打开另外一个命令行终端，输入如下命令。
rosrun topic_example subscribe_node启动完subscribe_node节点后，可以在终端中看到打印信息不断输出，如图1-15所示。这就说明订阅节点已经正常启动，并不断从chatter话题订阅消息数据了。
图1-15 启动subscribe_node节点订阅数据
到这里，我们编写的话题通信所包含的消息发布节点和订阅节点就大功告成了。虽然这个程序很简短，但是涵盖了一个ROS功能包基本的结构，为了让后面的学习能跟得上，这个例程大家一定要理解并掌握。
上面介绍了两个ros节点之间通过话题发布与订阅消息的方式通信，现在就介绍ros节点之间通信的另外一种方式，即服务方式。话题通信作为第一个入门例程，直接使用ROS标准的消息类型std_msgs::String帮助大家快速入门。这里的服务通信例程给大家增加一点难度，教大家使用自定义的消息类型。这里就以实现两个整数求和为例来讨论服务通信，client端节点向server端节点发送a、b的请求，server端节点返回响应sum=a+b给client端节点，具体过程如图1-11所示。
1.创建功能包
在~/catkin_ws/src/目录下新建功能包service_example，并在创建时显式地指明依赖roscpp和std_msgs，依赖std_msgs将作为基本数据类型用于我们自定义服务类型的封装，输入如下命令。
cd ~/catkin_ws/src/
#创建功能包service_example时，显式地指明依赖roscpp和std_msgs，
#依赖会被默认写到功能包的CMakeLists.txt和package.xml中
catkin_create_pkg service_example roscpp std_msgs2.自定义服务消息类型
前面已经介绍过可以利用ROS标准的消息类型来封装自定义的消息类型，这里将在功能包service_example中封装自定义服务消息类型。服务类型的定义文件都是以*.srv为扩展名，并且被放在功能包的srv/文件夹下。
（1）编写类型定义文件
首先，在功能包service_example目录下新建srv目录，然后在service_example/srv/目录中创建AddTwoInts.srv文件，文件内容见代码清单1-4。
代码清单1-4 创建AddTwoInts.srv文件
1 int64 a
2 int64 b
3 ---
4 int64 sum（2）设置类型编译配置
定义好我们的服务消息类型后，要想让该服务消息类型能在C++、Python等代码中被使用，必须要做相应的编译与运行配置。编译依赖message_generation，运行依赖message_runtime。
打开功能包中的CMakeLists.txt文件，找到代码清单1-5所示的代码段，将依赖message_generation添加进去，添加后的代码如代码清单1-6所示。
代码清单1-5 依赖添加前的CMakeLists.txt
1 find_package(catkin REQUIRED COMPONENTS
2 roscpp
3 std_msgs
4 )代码清单1-6 依赖添加后的CMakeLists.txt
1 find_package(catkin REQUIRED COMPONENTS
2 roscpp
3 std_msgs
4 message_generation
5 )继续在CMakeLists.txt文件中找到代码清单1-7所示的代码段，去掉这段代码的#注释，将自己编写的类型定义文件AddTwoInts.srv填入，修改好后的代码如代码清单1-8所示。
代码清单1-7 服务类型添加前的CMakeLists.txt
1 # add_service_files(
2 # FILES
3 # Service1.srv
4 # Service2.srv
5 # )代码清单1-8 服务类型添加后的CMakeLists.txt
1 add_service_files(
2 FILES
3 AddTwoInts.srv
4 )再继续在CMakeLists.txt文件中找到代码清单1-9所示的代码段，去掉这段代码的#注释，generate_messages的作用是自动创建我们自定义的消息类型*.msg、服务类型*.srv和动作类型*.action相对应的*.h，由于我们定义的服务消息类型使用了std_msgs中的int64基本类型，所以必须向generate_messages指明该依赖，修改好后的代码如代码清单1-10所示。
代码清单1-9 消息创建依赖添加前的CMakeLists.txt
1 # generate_messages(
2 # DEPENDENCIES
3 # std_msgs
4 # )代码清单1-10 消息创建依赖添加后的CMakeLists.txt
1 generate_messages(
2 DEPENDENCIES
3 std_msgs
4 )然后打开功能包中的package.xml文件，将代码清单1-11中的这三句依赖填入。
代码清单1-11 在package.xml添加依赖
1 <build_depend>message_generation</build_depend>
2 <build_export_depend>message_generation</build_export_depend>
3 <exec_depend>message_runtime</exec_depend>（3）检测新建的服务消息类型
做好了上面类型定义的编译配置后，就可以用命令检测新建的消息类型是否可以被ROS系统自动识别。前面已经说过，消息类型通过功能包和子类型名共同标识，用下面的命令进行检测。
rossrv show service_example/AddTwoInts如果能正确输出类型的数据结构，就说明新建的消息类型成功了，即可以被ROS系统自动识别到。
3.编写功能包源码
功能包中需要编写两个独立可执行的节点，一个节点是用来发起请求的client端，另一个节点是用来响应请求的server端，所以需要在新建的功能包service_example/src/目录下新建server_node.cpp和client_node.cpp两个文件。首先来看server节点，server_node.cpp代码内容见代码清单1-12。
代码清单1-12 服务节点server_node.cpp
1 #include "ros/ros.h"
2 #include "service_example/AddTwoInts.h"
3
4 bool add_execute(service_example::AddTwoInts::Request &req,
5 service_example::AddTwoInts::Response &res)
6 {
7 res.sum = req.a + req.b;
8 ROS_INFO("recieve request: a=%ld,b=%ld",(long int)req.a,(long int)req.b);
9 ROS_INFO("send response: sum=%ld",(long int)res.sum);
10 return true;
11 }
12
13 int main(int argc,char **argv)
14 {
15 ros::init(argc,argv,"server_node");
16 ros::NodeHandle nh;
17
18 ros::ServiceServer service = nh.advertiseService("add_two_ints",add_execute);
19 ROS_INFO("service is ready!!!");
20 ros::spin();
21
22 return 0;
23 }第1行，包含ROS的C++客户端roscpp的头文件，必须包含的头文件，就不多说了。
第2行，service_example/AddTwoInts.h是由编译系统自动根据我们的功能包和在功能包中创建的*.srv文件生成的对应头文件，包含这个头文件，程序中就可以使用我们自定义的服务消息类型了。
第4~11行，这个函数实现两个int64整数求和的服务，两个int64值从request获取，返回求和结果装入response里，request与response的具体数据类型都在前面创建的*.srv文件中被定义，这个函数返回值为bool型。
第15~16行，初始化ros节点并指明节点的名称，声明一个ros节点的句柄，,就不多说了。
第18行，这一句是创建服务，并将服务加入到ROS网络中，并且这个服务在ROS网络中以名称add_two_ints唯一标识，以便于其他节点通过服务名称进行请求。
第20行，这一句话让程序进入自循环的挂起状态，从而让程序以最大效率接收客户端的请求并调用回调函数，就不多说了。
其实将服务节点的代码稍作修改，就能得到客户端节点。接着来看看client节点，client_node.cpp代码内容见代码清单1-13。
代码清单1-13 客户端节点client_node.cpp
1 #include "ros/ros.h"
2 #include "service_example/AddTwoInts.h"
3
4 #include <iostream>
5
6 int main(int argc,char **argv)
7 {
8 ros::init(argc,argv,"client_node");
9 ros::NodeHandle nh;
10
11 ros::ServiceClient client =
12 nh.serviceClient<service_example::AddTwoInts>("add_two_ints");
13 service_example::AddTwoInts srv;
14
15 while(ros::ok())
16 {
17 long int a_in,b_in;
18 std::cout<<"please input a and b:";
19 std::cin>>a_in>>b_in;
20
21 srv.request.a = a_in;
22 srv.request.b = b_in;
23 if(client.call(srv))
24 {
25 ROS_INFO("sum=%ld",(long int)srv.response.sum);
26 }
27 else
28 {
29 ROS_INFO("failed to call service add_two_ints");
30 }
31 }
32 return 0;
33 }在server节点已经解释过的类似代码，就不再赘述了。这里重点解释一下前面没遇到过的代码。
第11~12行，这一句创建client对象，用来向ROS网络中名称为add_two_ints的service发起请求。
第13行，定义了一个service_example::AddTwoInts服务消息类型的对象，该对象中的成员正是我们在*.srv文件中定义的a、b、sum，我们将待请求的数据填充到数据成员a、b，请求成功后返回结果会被自动填充到数据成员sum中。
第23行，这一句便是通过client的方法call来向service发起请求，请求传入的参数srv在上面已经介绍过了。
4.配置和编译功能包
创建功能包service_example时，显式地指明了依赖roscpp和std_msgs，依赖已经被默认写到功能包的CMakeLists.txt和package.xml中，并且在功能包中创建*.srv服务类型时已经对服务的编译与运行做了相关配置，所以只需要在CMakeLists.txt文件的末尾行加入以下几句用于声明可执行文件就可以了，见代码清单1-14。
代码清单1-14 在CMakeLists.txt添加编译项
1 add_executable(server_node src/server_node.cpp)
2 target_link_libraries(server_node ${catkin_LIBRARIES})
3 add_dependencies(server_node service_example_gencpp)
4
5 add_executable(client_node src/client_node.cpp)
6 target_link_libraries(client_node ${catkin_LIBRARIES})
7 add_dependencies(client_node service_example_gencpp)add_executable用于创建可执行文件，也就是将源码编译成可执行文件；target_link_libraries用于连接可执行文件运行时需要的依赖库。add_dependencies用于声明可执行文件的依赖项，由于我们自定义了*.srv，service_example_gencpp的作用就是让编译系统自动根据我们的功能包和在功能包中创建的*.srv文件生成对应头文件和库文件，service_example_gencpp这个名称是由功能包名称service_example加上_gencpp后缀而来的，后缀很好理解：生成C++文件就是_gencpp，生成Python文件就是_genpy。
修改好CMakeLists.txt文件末尾的编译配置后，就可以编译我们的功能包了，编译命令如下。其中catkin_make命令后面的参数DCATKIN_WHITELIST_PACKAGES指定需要编译的功能包，直接使用cakin_make的话就是编译工作空间中的所有功能包。如果新建的功能包加入工作空间后，使用catkin_make执行编译无响应，需要把catkin_ws/build/CATKIN_IGNORE删除或者直接删除catkin_ws/build后重新执行编译。
cd ~/catkin_ws/
catkin_make -DCATKIN_WHITELIST_PACKAGES="service_example"5.启动功能包中的节点
首先，需要用roscore命令来启动ROS节点管理器，ROS节点管理器是所有节点运行的基础。打开命令行终端，输入如下命令。
roscore然后，就可以用rosrun命令来启动功能包service_example中的节点server_node，为别的节点提供两个整数求和的服务，打开另外一个命令行终端，输入如下命令。
rosrun service_example server_node启动完server_node节点后，可以在终端中看到服务已就绪的打印信息输出，如图1-16所示。这就说明服务节点已经正常启动，为两个整数求和的服务已经就绪，只要客户端发起请求就能立马给出响应。
图1-16 启动server_node节点提供服务
最后，用rosrun命令来启动功能包service_example中的节点client_node，向server_node发起请求。打开另外一个命令行终端，输入如下命令。
rosrun service_example client_node启动完client_node节点后，按照终端输出提示信息，用键盘键入两个整数，以空格分割，输入完毕后回车。如果看到输出信息sum=xxx，就说明client节点向server端发起的请求得到了响应，打印出来的sum就是响应结果，这样就完成了一次服务请求的通信过程，如图1-17所示。
图1-17 启动client_node节点发起请求
到这里，我们编写的服务通信所包含的服务端节点和客户端节点就大功告成了。这里需要掌握自定义消息类型和服务通信请求过程实现两个知识点。
上面介绍了两个ros节点之间通过服务与请求的通信方式，现在就介绍ros节点之间通信的另外一种方式，即动作方式。跟服务通信方式类似，动作通信方式只是在响应中多了一个反馈机制。跟服务通信例程一样，这里的动作通信例程也是使用自定义消息类型。这里以实现倒计数器为例，动作客户端节点向动作服务端节点发送倒计数的请求，动作服务端节点执行递减计数任务，并给出反馈和结果，具体过程如图1-12所示。
1.创建功能包
在~/catkin_ws/src/目录下新建功能包action_example，并在创建时显式地指明依赖roscpp、std_msgs、actionlib_msgs、actionlib和message_generation，依赖std_msgs和actionlib_msgs将作为基本数据类型用于我们自定义动作类型的封装，依赖actionlib将用于节点中创建动作服务和动作客户端，message_generation是创建自定义消息类型的工具，输入如下命令。
cd ~/catkin_ws/src/
#创建功能包action_example时，显式地指明依赖roscpp、std_msgs等
#依赖会被默认写到功能包的CMakeLists.txt和package.xml中
catkin_create_pkg action_example roscpp std_msgs actionlib_msgs actionlib message_generation2.自定义动作消息类型
前面已经介绍过封装自己的服务消息类型了，这里按类似方法将在功能包action_example中封装自定义的动作消息类型。动作类型的定义文件都是以*.action为扩展名，并且被放在功能包的action/文件夹下。
（1）编写类型定义文件
首先，在功能包action_example目录下新建action目录，然后在action_example/action/目录中创建CountDown.action文件，文件内容见代码清单1-15。这里对动作消息的数据结构做一个分析，动作消息分为目标、结果和反馈三个部分，每个部分的定义内容用3个连续的短线分隔。每个部分内部可以定义一个或多个数据成员，根据需要定义。
代码清单1-15 创建CountDown.action文件
1 #goal define
2 int32 target_number
3 int32 target_step
4 ---
5 #result define
6 bool finish
7 ---
8 #feedback define
9 float32 count_percent
10 int32 count_current（2）设置类型编译配置
定义好动作消息类型后，要想让该动作消息类型能在C++、python等代码中被使用，必须要做相应的编译与运行配置。编译依赖message_generation已经在新建功能包时显示指定了，运行依赖message_runtime需要手动添加一下。
打开功能包中的CMakeLists.txt文件，找到代码清单1-16所示的代码段，去掉注释如代码清单1-17所示，将依赖Boost放出来，因为代码中用到了Boost库。
代码清单1-16 Boost依赖添加前的CMakeLists.txt
# find_package(Boost REQUIRED COMPONENTS system)代码清单1-17 Boost依赖添加后的CMakeLists.txt
find_package(Boost REQUIRED COMPONENTS system)继续在CMakeLists.txt文件中找到代码清单1-18所示的代码段，去掉这段代码的#注释，将自己编写的类型定义文件CountDown.action填入，修改好后的代码如代码清单1-19所示。
代码清单1-18 动作类型添加前的CMakeLists.txt
1 # add_action_files(
2 # FILES
3 # Action1.action
4 # Action2.action
5 # )代码清单1-19 动作类型添加后的CMakeLists.txt
1 add_action_files(
2 FILES
3 CountDown.action
4 )再继续在CMakeLists.txt文件中找到代码清单1-20所示的代码段，去掉这段代码的#注释，generate_messages的作用是自动创建我们自定义的消息类型*.msg、服务类型*.srv和动作类型*.action相对应的*.h。由于我们定义的动作消息类型使用了std_msgs中的基本类型和必须使用的actionlib_msgs中的类型，所以必须向generate_messages指明该依赖，修改好后的代码如代码清单1-21所示。
代码清单1-20 消息创建依赖添加前的CMakeLists.txt
1 # generate_messages(
2 # DEPENDENCIES
3 # actionlib_msgs
4 # std_msgs
5 # )代码清单1-21 消息创建依赖添加后的CMakeLists.txt
1 generate_messages(
2 DEPENDENCIES
3 actionlib_msgs
4 std_msgs
5 )然后打开功能包中的package.xml文件，将代码清单1-22中的这句依赖填入。
代码清单1-22 在package.xml添加依赖
<exec_depend>message_runtime</exec_depend>做好了上面类型定义的编译配置，一旦后面对功能包进行编译后，ROS系统将会生成我们自定义动作类型的调用头文件，同时还会产生很多供调用的配套子类型，这一点特别要拿来说明一下，因为后面程序中会使用这些类型，往往初学者搞不懂这些没见过的子类型是来自哪里。本实例中CountDown.action经过编译会产生对应的*.msg和*.h文件，如图1-18所示。在程序中，只需要引用action_example/CountDownAction.h头文件，就能使用自定义动作类型以及配套的子类型。
图1-18 自定义动作消息类型创建过程
3.编写功能包源码
功能包中需要编写两个独立可执行的节点，一个节点是用来发起目标的动作客户端，另一个节点是用来执行目标任务的动作服务端，所以需要在新建的功能包action_example/src/目录下新建action_server_node.cpp和action_client_node.cpp两个文件。首先来看动作服务端节点，action_server_node.cpp代码内容见代码清单1-23。
代码清单1-23 动作服务节点action_server_node.cpp
1 #include "ros/ros.h"
2 #include "actionlib/server/simple_action_server.h"
3 #include "action_example/CountDownAction.h"
4
5 #include <string>
6 #include <boost/bind.hpp>
7
8 class ActionServer
9 {
10 private:
11 ros::NodeHandle nh_;
12 actionlib::SimpleActionServer<action_example::CountDownAction> as_;
13
14 action_example::CountDownGoal goal_;
15 action_example::CountDownResult result_;
16 action_example::CountDownFeedback feedback_;
17
18 public:
19 ActionServer(std::string name):
20 as_(nh_,name,boost::bind(&ActionServer::executeCB,this,_1),false)
21 {
22 as_.start();
23 ROS_INFO("action server started!");
24 }
25 ~ActionServer(void){}
26 void executeCB(const action_example::CountDownGoalConstPtr &goal)
27 {
28 ros::Rate r(1);
29 goal_.target_number=goal->target_number;
30 goal_.target_step=goal->target_step;
31 ROS_INFO("get goal:[%d,%d]",goal_.target_number,goal_.target_step);
32
33 int count_num=goal_.target_number;
34 int count_step=goal_.target_step;
35 bool flag=true;
36 for(int i=count_num;i>0;i=i-count_step)
37 {
38 if(as_.isPreemptRequested() || !ros::ok())
39 {
40 as_.setPreempted();
41 flag=false;
42 ROS_INFO("Preempted");
43 break;
44 }
45 feedback_.count_percent=1.0*i/count_num;
46 feedback_.count_current=i;
47 as_.publishFeedback(feedback_);
48
49 r.sleep();
50 }
51 if(flag)
52 {
53 result_.finish=true;
54 as_.setSucceeded(result_);
55 ROS_INFO("Succeeded");
56 }
57 }
58 };
59
59 int main(int argc, char** argv)
60 {
61 ros::init(argc, argv, "action_server_node");
62
63 ActionServer my_action_server("/count_down");
64 ros::spin();
65 return 0;
66 }本实例除了要掌握动作消息通信的实现外，还需要会用面向对象的思想来编写程序。在后续章节所涉及的复杂项目基本上都采用面向对象的编程方式，将复杂的功能模块封装到类中，方便调用和管理。
第1行，包含ROS的C++客户端roscpp的头文件。
第2行，创建动作服务端需要的头文件。
第3行，是引用我们自定义动作消息类型头文件。
第5～6行，是使用C++的string和boost库的头文件。
第8~58行，是定义我们的类ActionServer，将动作服务端及任务处理逻辑封装在这个类里面。其中第11行，声明一个ros节点的句柄。第12行，创建一个动作服务端对象as_，后面的操作都通过调用这个对象的方法来实现。第14～16行，新建三个变量用于动作服务端与客户端交互时的数据缓存。第19～24行，是类的构造函数，带一个字符串的传参，并且对动作服务端对象as_做初始化，初始化采用boost方法将executeCB回调函数进行关联，当动作服务端收到目标后会自动跳到executeCB回调函数，具体任务逻辑在里面实现。第26～57行，是executeCB回调函数的具体实现，其实逻辑大致就是获取目标goal的值，这里goal有两个成员，一个是计数值，另一个是计数的步数；然后执行递减操作，并且输出反馈feedback，这里feedback也有两个成员，一个是执行百分比，另一个是计数当前值；最后是执行完成了，输出结果result值。
第61行，这个是main函数里面必须有的，初始化ros节点并指明节点的名称。
第63行，创建一个类ActionServer的实例对象。实例对象初始参数是/count_down，这个参数是动作服务访问的标识名，也就是动作客户端可以利用/count_down标识名与动作服务端进行连接。
其实将动作服务端节点的代码稍作修改，就能得到动作客户端节点。接着来看看动作客户端节点，action_client_node.cpp代码内容见代码清单1-24。
代码清单1-24 动作客户端节点action_client_node.cpp
1 #include "ros/ros.h"
2 #include "actionlib/client/simple_action_client.h"
3 #include "actionlib/client/terminal_state.h"
4 #include "action_example/CountDownAction.h"
5
6 void doneCB(const actionlib::SimpleClientGoalState& state,
7 const action_example::CountDownResultConstPtr& result)
8 {
9 ROS_INFO("done");
10 ros::shutdown();
11 }
12 void activeCB()
13 {
14 ROS_INFO("active");
15 }
16 void feedbackCB(const action_example::CountDownFeedbackConstPtr& feedback)
17 {
18 ROS_INFO("feedback:[%f,%d]",
feedback->count_percent,
feedback->count_current);
19 }
20 int main(int argc, char **argv)
21 {
22 ros::init(argc, argv, "action_client_node");
23
24 actionlib::SimpleActionClient<action_example::CountDownAction> ac("/count_down",true);
25
26 ROS_INFO("wait for action server to start!");
27 ac.waitForServer();
28
29 action_example::CountDownGoal goal;
30 std::cout<<"please input target_number and target_step:"<<std::endl;
31 std::cin>>goal.target_number>>goal.target_step;
32
33 ac.sendGoal(goal,&doneCB,&activeCB,&feedbackCB);
34
35 ros::spin();
36 return 0;
37 }在动作服务端节点已经解释过的类似代码，就不再赘述了。这里重点解释一下前面没遇到过的代码。
第2～3行，是创建动作客户端需要的头文件。
第6～19行，定义了三个函数，分别用来处理结果、开始、反馈的消息。
第24行，创建一个动作客户端对象ac，使用动作服务端的标识名/count_down作为初始化参数。
第27行，是客户端ac与动作服务端建立连接同步的过程。
第29～31行，获取键盘值来填充goal的值。
第33行，客户端ac调用发送目标成员函数，向动作服务端发起目标，并且关联三个回调函数用来处理反馈和结果。
4.配置和编译功能包
创建功能包action_example时，显式地指明了依赖roscpp、std_msgs等，依赖已经被默认写到功能包的CMakeLists.txt和package.xml中，并且在功能包中创建*.action动作类型时已经对动作的编译与运行做了相关配置，所以只需要在CMakeLists.txt文件的末尾行加入以下几句用于声明可执行文件就可以了，见代码清单1-25。关于这些编译配置前面已经讲过，就不多说了。
代码清单1-25 在CMakeLists.txt添加编译项
1 add_executable(action_server_node src/action_server_node.cpp)
2 add_dependencies(action_server_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
3 target_link_libraries(action_server_node ${catkin_LIBRARIES})
4
5 add_executable(action_client_node src/action_client_node.cpp)
6 add_dependencies(action_client_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
7 target_link_libraries(action_client_node ${catkin_LIBRARIES})修改好CMakeLists.txt文件末尾的编译配置后，就可以编译我们的功能包了，编译命令如下。其中catkin_make命令后面的参数DCATKIN_WHITELIST_PACKAGES指定需要编译的功能包，直接使用cakin_make的话就是编译工作空间中的所有功能包。如果新建的功能包加入工作空间后，使用catkin_make执行编译无响应，需要把catkin_ws/build/CATKIN_IGNORE删除或者直接删除catkin_ws/build后重新执行编译。
cd ~/catkin_ws/
catkin_make -DCATKIN_WHITELIST_PACKAGES="action_example"5.启动功能包中的节点
首先，需要用roscore命令来启动ROS节点管理器，ROS节点管理器是所有节点运行的基础。打开命令行终端，输入如下命令。
roscore然后可以用rosrun命令来启动功能包action_example中的节点action_server_node，为别的节点提供动作的服务。打开另外一个命令行终端，输入如下命令。
rosrun action_example action_server_node启动完action_server_node节点后，可以在终端中看到动作服务已就绪的打印信息输出，如图1-19所示。这就说明动作服务节点已经正常启动，只要客户端发起目标就能开始执行目标和反馈。
图1-19 启动action_server_node节点提供动作服务
最后，用rosrun命令来启动功能包action_example中的节点action_client_node，向action_server_node发起目标。打开另外一个命令行终端，输入如下命令。
rosrun action_example action_client_node启动完action_client_node节点后，按照终端输出提示信息，用键盘键入两个整数，以空格分割，输入完毕后回车。如果看到输出反馈信息，就说明动作客户端节点向动作服务端发起的目标已经开始执行，目标执行完成后，客户端程序自动结束，这样就完成了一次动作目标请求的通信过程，如图1-20所示。
图1-20 启动action_client_node节点发起动作目标
到这里，我们编写的动作通信所包含的动作服务端节点和动作客户端节点就大功告成了。这里除了需要掌握自定义消息类型和动作通信过程实现两个知识点外，还需要掌握面向对象的编程思想。
Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation
Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation
【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.