《动手学ROS2》4.10服务实现(C++)
本系列教程作者：小鱼
公众号：鱼香ROS
QQ交流群：139707339
教学视频地址：小鱼的B站
完整文档地址：鱼香ROS官网
版权声明：如非允许禁止转载与商业用途。
4.5.3 C++服务通信实现(张三买书)
看到张三买书就突然想起华强买瓜，但张三是真心买书不是存心找茬。开始编写代码之前，我们先梳理一下买书任务流程。
1.任务流程
一句话：张三拿多少钱钱给王二，王二凑够多少个章节的艳娘传奇给他
2.服务端（王二）实现
首先是作为二手书提供者的服务端王二节点代码的编写。
2.1 创建C++服务通信服务端的步骤
-
导入服务接口
-
创建服务端回调函数
-
声明并创建服务端
-
编写回调函数逻辑处理请求
2.2 添加服务接口与依赖
2.2.1 添加依赖
添加依赖是为了让程序能够在编译和运行时找到对应的接口
因为village_wang的包类型是ament_cmake，故需要进行以下两步操作：
第一步修改package.xml
加入下面的代码（告诉colcon，编译之前要确保有village_interfaces存在）
<depend>village_interfacesdepend>
第二步修改和CMakeLists.txt
在CMakeLists.txt中加入下面一行代码
find_package(village_interfaces REQUIRED)
find_package是cmake的语法，用于查找库。找到后，还需要将其和可执行文件链接起来
所以还需要修改ament_target_dependencies，在其中添加village_interfaces。
ament_target_dependencies(wang2_node
rclcpp
village_interfaces
)
2.2.2 添加服务接口
对于C++来说，添加服务接口只需在程序中引入对应的头文件即可。
这个头文件就是我们
SellNovel.srv生成的头文件
#include "village_interfaces/srv/sell_novel.hpp"
2.3 声明回调函数
2.3.1 声明回调函数
添加完服务接口接着就可以声明一个卖书请求回调函数。
// 声明一个回调函数，当收到买书请求时调用该函数，用于处理数据
void sell_book_callback(const village_interfaces::srv::SellNovel::Request::SharedPtr request,
const village_interfaces::srv::SellNovel::Response::SharedPtr response)
{
}
2.3.2 存书的队列（可以理解为书库）
再创建一个队列，用于存放自己看过的二手书，创建队列需要用到queue容器，所以我们先用#include 在程序开头引入该容器，再在代码中添加下面这句话。
//创建一个小说章节队列
std::queue novels_queue;
2.3.3 死锁
当张三请求王二买二手书的时候，假如王二手里书的数量不足，王二就等攒够了对应数量的书再返回给张三。
等待攒够章节的操作需要在卖书服务函数中阻塞当前线程，阻塞后王二就收不到李四写的小说了，这样一来就会造成一个很尴尬的情景：
在卖书服务回调函数中等着书库（队列）里小说章节数量满足张三需求，接收小说的程序等着这边的卖书回调函数结束，好把书放进书库（队列）里。
这种互相等待的情况，我们称之为死锁。那如何解决呢？
2.3.4 多线程
大家可能会问，为啥接收小说的程序不能自己单独干，非要等待服务回调函数结束才把书放到书库，不能收到书就把书直接放到书库吗？
原因是ROS2默认是单线程的，同时只有一个线程在跑，大家都是顺序执行，你干完我干，一条线下去。
所以为了解决这个问题，我们可以使用多线程，即每次收到服务请求后，单独开一个线程来处理，不影响其他部分。
2.3.5 回调函数组
ROS2中要使用多线程执行器和回调组来实现多线程，我们先在SingleDogNode中声明一个回调组成员变量。
// 声明一个服务回调组
rclcpp::CallbackGroup::SharedPtr callback_group_service_;
完成 声明之后，我们的SingleDogNode新增内容如下：
2.3.6 最终结果
class SingleDogNode : public rclcpp::Node
{
public:
// 构造函数
SingleDogNode(std::string name) : Node(name)
{
}
private:
// 声明一个服务回调组
rclcpp::CallbackGroup::SharedPtr callback_group_service_;
//创建一个小说章节队列
std::queue novels_queue;
// 声明一个服务端
rclcpp::Service::SharedPtr server_;
// 声明一个回调函数，当收到买书请求时调用该函数，用于处理数据
void sell_book_callback(const village_interfaces::srv::SellNovel::Request::SharedPtr request,
const village_interfaces::srv::SellNovel::Response::SharedPtr response)
{
//对请求数据进行处理
}
};
2.4 实例化服务端并编写回调函数处理请求
2.4.1 实例化回调组
在ROS2中，回调函数组也是一个对象，通过实例化create_callback_group类即可创建一个callback_group_service的对象。
在SingleDogNode的构造函数中添加下面这行代码，即可完成实例化
callback_group_service_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
2.4.2 声明并实例化服务端
我们使用成员函数作为回调函数，这里要根据回调函数中参数个数，设置占位符，即告诉编译器，这个函数需要传入的参数个数。
在之前订阅话题的回调函数中，我们已经用到过一次了，因为话题回调只有一个参数，所以只需要一个占位符，这里服务的回调是两个参数，所以要设置两个
using std::placeholders::_1;
using std::placeholders::_2;
在private:下声明服务端
// 声明一个服务端
rclcpp::Service::SharedPtr server_;
在构造函数中实例化服务端
// 实例化卖二手书的服务
server_ = this->create_service("sell_novel",
std::bind(&SingleDogNode::sell_book_callback,this,_1,_2),
rmw_qos_profile_services_default,
callback_group_service_);
实例化服务端可以直接使用create_service函数，该函数是一个模版函数，需要输入要创建的服务类型，这里我们使用的是
"sell_novel"服务名称，没啥好说的，要唯一哦，因为服务只能有一个std::bind(&SingleDogNode::sell_book_callback,this,_1,_2)回调函数，这里指向了我们2.3.1中我们声明的sell_book_callbackrmw_qos_profile_services_default通信质量，这里使用服务默认的通信质量callback_group_service_，回调组，我们前面创建回调组就是在这里使用的，告诉ROS2，当你要调用回调函数处理请求时，请把它放到单独线程的回调组中
2.4.4 编写回调函数
现在我们开始编写回调函数，这里属于重点部分。先把整个代码放一下。
// 声明一个回调函数，当收到买书请求时调用该函数，用于处理数据
void sell_book_callback(const village_interfaces::srv::SellNovel::Request::SharedPtr request,
const village_interfaces::srv::SellNovel::Response::SharedPtr response)
{
RCLCPP_INFO(this->get_logger(), "收到一个买书请求，一共给了%d钱",request->money);
unsigned int novelsNum = request->money*1; //应给小说数量，一块钱一章
//判断当前书库里书的数量是否满足张三要买的数量，不够则进入等待函数
if(novels_queue.size()get_logger(), "当前艳娘传奇章节存量为%d：不能满足需求,开始等待",novels_queue.size());
// 设置rate周期为1s，代表1s检查一次
rclcpp::Rate loop_rate(1);
//当书库里小说数量小于请求数量时一直循环
while (novels_queue.size()get_logger(), "程序被终止了");
return ;
}
//打印一下当前的章节数量和缺少的数量
RCLCPP_INFO(this->get_logger(), "等待中，目前已有%d章，还差%d章",novels_queue.size(),novelsNum-novels_queue.size());
//rate.sleep()让整个循环1s运行一次
loop_rate.sleep();
}
}
// 章节数量满足需求了
RCLCPP_INFO(this->get_logger(), "当前艳娘传奇章节存量为%d：已经满足需求",novels_queue.size());
//一本本把书取出来，放进请求响应对象response中
for(unsigned int i =0 ;inovels.push_back(novels_queue.front());
novels_queue.pop();
}
}
当收到请求时，先计算一下应该给张三多少书novelsNum，然后判断书库里书的数量够不够，不够则进入攒书程序。如果够或者攒够了就把书放到服务响应对象里，返回给张三。
你可能有一些疑问，我们并没有写把书放进书库（队列novels_queue）的程序呀，是的，这里我们还需要修改一下话题回调函数，增加了一行代码，将小说放到书库里novels_queue.push(msg->data);
// 收到话题数据的回调函数
void topic_callback(const std_msgs::msg::String::SharedPtr msg){
// 新建一张人民币
std_msgs::msg::UInt32 money;
money.data = 10;
// 发送人民币给李四
pub_->publish(money);
RCLCPP_INFO(this->get_logger(), "王二：我收到了：'%s' ，并给了李四：%d 元的稿费", msg->data.c_str(),money.data);
//将小说放入novels_queue中
novels_queue.push(msg->data);
};
2.5 修改main函数
因为我们要让整个程序变成多线程的，所以我们要把节点的执行器变成多线程执行器。
修改一下main函数，新建一个多线程执行器，添加王二节点并spin,完整代码如下：
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个Wang2的节点*/
auto node = std::make_shared("wang2");
/* 运行节点，并检测退出信号*/
rclcpp::executors::MultiThreadedExecutor exector;
exector.add_node(node);
exector.spin();
rclcpp::shutdown();
return 0;
}
王二节点完整代码见：https://fishros.com/code/ros2/ros2_town/village_wang/src/wang2.cpp
2.6 编译
在工作空间下：输入下面的指令
colcon build --packages-select village_wang
2.7 运行测试
2.7.1 source环境
打开vscode终端，输入
source install/setup.bash
2.7.2 运行王二节点
ros2 run village_wang wang2_node
2.7.3 使用命令行发送买书请求
切分一个终端，source
source install/setup.bash
查看一下服务列表
ros2@ubuntu:~/code/town_ws$ ros2 service list -t
/sell_book [village_interfaces/srv/SellNovel]
手动发送买书请求
ros2 service call /sell_book village_interfaces/srv/SellNovel "{money: 5}"
2.7.4 启动李四写书
观察以上结果可以发现，我们并没有买到书，因为王二这里也没有，这时候就需要我们来启动李四节点来写书了。
再切分一个终端出来：
source install/setup.bash
启动李四写书
ros2 run village_li li4_node
2.7.5 结果展示
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VKDVzbo7-1636351122758)(http://fishros.com/d2lros2foxy/chapt4/4.10%E6%9C%8D%E5%8A%A1%E5%AE%9E%E7%8E%B0%28C%2B%2B%29/imgs/image-20210831124712850.png)]
3.客户端（张三）实现
编写完服务端的程序，接下来我们就可以编写客户端张三了。
3.1 编写ROS2服务通信客户端步骤
- 导入服务接口
- 创建请求结果接收回调函数
- 声明并创建客户端
- 编写结果接收逻辑
- 调用客户端发送请求
3.2 添加服务接口与依赖
3.2.1 创建客户端节点
因为张家村和张三之前不存在，这里我们需要新创建出来，命令如下：
打开终端到src文件夹下：
ros2 pkg create village_zhang --build-type ament_cmake --dependencies rclcpp
然后在src/village_zhang/src目录下新建zhang3.cpp
完成后目录结构如下：
3.2.2 创建客户端节点
因为张三要找王二请求小说，所以一定依赖通信接口village_interfaces
添加依赖是为了能够让程序在编译和运行时找到对应的接口
因为village_zhang是包类型是ament_cmake，与上面一样需要两步操作.
第一步修改package.xml
加入下面的代码（告诉colcon，编译之前要确保有village_interfaces存在）
<depend>village_interfacesdepend>
第二步修改和CMakeLists.txt
在CMakeLists.txt中加入下面一行代码
find_package(village_interfaces REQUIRED)
find_package是cmake的语法，用于查找库。找到后，还需要将其和可执行文件链接起来
接着添加可执行文件
add_executable(zhang3_node src/zhang3.cpp)
上面找到库之后，将其与可执行文件链接起来，还需要修改ament_target_dependencies，在其中添加rclcpp 和 village_interfaces。
ament_target_dependencies(zhang3_node
rclcpp
village_interfaces
)
3.3 创建张三节点、服务客户端、请求函数和请求结果回调函数
3.3.1 创建客户端
接着我们就可以编写客户端了，穷光蛋张三也是C++，我们可以参考王二的代码，建立一个C++节点的基本的框架。
#include "rclcpp/rclcpp.hpp"
#include "village_interfaces/srv/sell_novel.hpp"
// 提前声明的占位符，留着创建客户端的时候用
using std::placeholders::_1;
/*
创建一个类节点，名字叫做PoorManNode,继承自Node.
*/
class PoorManNode : public rclcpp::Node
{
public:
/* 构造函数 */
PoorManNode(std::string name) : Node(name)
{
// 打印一句自我介绍
RCLCPP_INFO(this->get_logger(), "大家好，我是得了穷病的张三.");
}
private:
};
/*主函数*/
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个Zhang3的节点*/
auto node = std::make_shared("zhang3");
/* 运行节点，并检测rclcpp状态*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
3.3.2 声明客户端
这里只是声明客户端，并指明这个客户端的类型是village_interfaces::srv::SellNovel
// 创建一个客户端
rclcpp::Client::SharedPtr client_;
3.3.3 创建请求函数和回调函数
在class里的public下新建两个函数，第一个函数用于买小说，第二个函数用于回调买小说的结果response。
/*买小说函数*/
void buy_novel()
{
RCLCPP_INFO(this->get_logger(), "买小说去喽");
}
/*接收小说-回调函数*/
void novels_callback(rclcpp::Client::SharedFuture response)
{
}
完整代码：
#include "rclcpp/rclcpp.hpp"
#include "village_interfaces/srv/sell_novel.hpp"
// 提前声明的占位符，留着创建客户端的时候用
using std::placeholders::_1;
/*
创建一个类节点，名字叫做PoorManNode,继承自Node.
*/
class PoorManNode : public rclcpp::Node
{
public:
/* 构造函数 */
PoorManNode() : Node("zhang3")
{
// 打印一句自我介绍
RCLCPP_INFO(this->get_logger(), "大家好，我是得了穷病的张三.");
}
/*买小说函数*/
void buy_novel()
{
RCLCPP_INFO(this->get_logger(), "买小说去喽");
}
/*接收小说-回调函数*/
void novels_callback(rclcpp::Client::SharedFuture response)
{
}
private:
// 创建一个客户端
rclcpp::Client::SharedPtr client_;
};
/*主函数*/
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个PoorManNode的节点*/
auto node = std::make_shared();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
3.4 实例化客户端、编写请求函数和回调函数
3.4.1 实例化客户端
这个我们很熟悉了，先声明再实例化，在构造函数里加一句话。
//实例化客户端
client_ = this->create_client("sell_novel");
实例化调用自身的create_client函数即可，这个函数依然是一个模板函数。
这里实例化的时候也要指明客户端的接口类型，同时指定要请求的服务的名称sell_novel.
3.4.2 编写请求函数buy_novel
编写请求函数but_novel()。
整个函数可以分为三个部分：
- 等待服务端上线
- 构造请求数据
- 发送异步请求
void buy_novel()
{
RCLCPP_INFO(this->get_logger(), "买小说去喽");
//1.等待服务端上线
while (!client_->wait_for_service(std::chrono::seconds(1)))
{
//等待时检测rclcpp的状态
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "等待服务的过程中被打断...");
return;
}
RCLCPP_INFO(this->get_logger(), "等待服务端上线中");
}
//2.构造请求的钱
auto request = std::make_shared();
//先来五块钱的看看好不好看
request->money = 5;
//3.发送异步请求，然后等待返回，返回时调用回调函数
client_->async_send_request(request,std::bind(&PoorManNode::novels_callback, this, _1));
};
结构还是很清晰的，等待服务端上线的时候我们使用的是client_->wait_for_service这个函数，该函数有一个参数是超时时间，小鱼这里设置成1s，如果服务端没有上线则一直等待。
第二部分构造请求的钱，C++还是一如既往的长，这里使用make_shared创建了一个指向village_interfaces::srv::SellNovel_Request的指针，并赋值给了requet。可以理解为创建了一个钱袋。
auto request = std::make_shared();
创建好钱袋（request）之后，我们开始往钱袋里装钱，这里装了5块钱。
//先来五块钱的看看好不好看
request->money = 5;
第三步发送请求
这一步我们和Python中一样，发送的是异步请求，并且设置了一个回调函数，意思是当我们请求（买书）成功时，请调用这个回调函数，把结果通过回调函数的参数传递过来。
关于回调函数前面提过好几次了，小鱼也在公众号上写了相关的文章，不明白的同学可以前往翻阅。
3.4.3 编写回调函数处理结果
接着编写回调函数novels_callback处理结果，完整的回调函数内容如下，也很简单，获取结果然后遍历结果打印。
注意函数的参数并不是
SellNovel的Response对象，而是SharedFuture，这个小鱼会写篇文章说一说。这里直接用就行，使用response的get()获取。
//创建接收到小说的回调函数
void novels_callback(rclcpp::Client::SharedFuture response)
{
auto result = response.get();
RCLCPP_INFO(this->get_logger(), "收到%d章的小说，现在开始按章节开读", result->novels.size());
for(std::string novel:result->novels)
{
//打印小说章节内容
RCLCPP_INFO(this->get_logger(), "%s", novel.c_str());
}
RCLCPP_INFO(this->get_logger(), "小说读完了，好刺激，写的真不错，好期待下面的章节呀！");
}
3.4.4 修改main函数调用请求函数
完成上面的工作之后还需要修改一下main函数来调用一下我们的买小说函数。
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
/*产生一个Zhang3的节点*/
auto node = std::make_shared();
node->buy_novel();
/* 运行节点，并检测rclcpp状态*/
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
完成代码参考：ros2_town/wang2.cpp · fishros
3.5 修改CmkeLists.txt并编译
3.5.1 添加指令
张三节点之前没有安装过，这里需要在CMakeLists.txt加入一个安装指令，将编译好的可执行文件，安装到install目录下。
install(TARGETS
zhang3_node
DESTINATION lib/${PROJECT_NAME}
)
3.5.2 编译
colcon build --packages-select village_zhang
4.测试
完成了张三和王二的客户端和服务端的程序后，我们就可以测试啦。
4.1运行张三
打开新终端，先source再运行张三
source install/setup.bash
ros2 run village_zhang zhang3_node
4.2 运行王二
切分终端，source并运行王二
source install/setup.bash
ros2 run village_wang wang2_node
4.3 运行李四写书
切分终端，source并运行李四
source install/setup.bash
ros2 run village_li li4_node
4.5 结果展示
4.测试
完成了张三和王二的客户端和服务端的程序后，我们就可以测试啦。
4.1运行张三
打开新终端，先source再运行张三
source install/setup.bash
ros2 run village_zhang zhang3_node
4.2 运行王二
切分终端，source并运行王二
source install/setup.bash
ros2 run village_wang wang2_node
4.3 运行李四写书
切分终端，source并运行李四
source install/setup.bash
ros2 run village_li li4_node
4.5 结果展示
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-SsQrAU4e-1636351496658)(http://d2lros2foxy.fishros.com/chapt4/4.10服务实现(C++)]/imgs/image-20210901201805844.png)
作者介绍：
我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚
初中学习编程，高中开始接触机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小智，一起交流技术，学习机器人