《动手学ROS2》4.10服务实现(C++)

本系列教程作者：小鱼
公众号：鱼香ROS
QQ交流群：139707339
教学视频地址：小鱼的B站
完整文档地址：鱼香ROS官网
版权声明：如非允许禁止转载与商业用途。

4.5.3 C++服务通信实现(张三买书)

看到张三买书就突然想起华强买瓜，但张三是真心买书不是存心找茬。开始编写代码之前，我们先梳理一下买书任务流程。

1.任务流程

一句话：张三拿多少钱钱给王二，王二凑够多少个章节的艳娘传奇给他

2.服务端（王二）实现

首先是作为二手书提供者的服务端王二节点代码的编写。

2.1 创建C++服务通信服务端的步骤

1. 导入服务接口

2. 创建服务端回调函数

3. 声明并创建服务端

4. 编写回调函数逻辑处理请求

2.2 添加服务接口与依赖

2.2.1 添加依赖

添加依赖是为了让程序能够在编译和运行时找到对应的接口

因为village_wang的包类型是ament_cmake，故需要进行以下两步操作：

第一步修改package.xml

加入下面的代码（告诉colcon，编译之前要确保有village_interfaces存在）

  <depend>village_interfaces</depend>

第二步修改和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 <queue>在程序开头引入该容器，再在代码中添加下面这句话。

//创建一个小说章节队列
std::queue<std::string>  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<std::string>  novels_queue;
    // 声明一个服务端
    rclcpp::Service<village_interfaces::srv::SellNovel>::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<village_interfaces::srv::SellNovel>::SharedPtr server_;

在构造函数中实例化服务端

// 实例化卖二手书的服务
server_ = this->create_service<village_interfaces::srv::SellNovel>("sell_novel",
                            std::bind(&SingleDogNode::sell_book_callback,this,_1,_2),
                            rmw_qos_profile_services_default,
                            callback_group_service_);

实例化服务端可以直接使用create_service函数，该函数是一个模版函数，需要输入要创建的服务类型，这里我们使用的是<village_interfaces::srv::SellNovel>，这个函数有四个参数需要输入,小鱼接下来进行一一介绍

• "sell_novel"服务名称，没啥好说的，要唯一哦，因为服务只能有一个
• std::bind(&SingleDogNode::sell_book_callback,this,_1,_2)回调函数，这里指向了我们2.3.1中我们声明的sell_book_callback
• rmw_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()<novelsNum)
        {
            RCLCPP_INFO(this->get_logger(), "当前艳娘传奇章节存量为%d：不能满足需求,开始等待",novels_queue.size());

            // 设置rate周期为1s，代表1s检查一次
            rclcpp::Rate loop_rate(1);

            //当书库里小说数量小于请求数量时一直循环
            while (novels_queue.size()<novelsNum)
            {
                //判断系统是否还在运行
                if(!rclcpp::ok())
                {
                    RCLCPP_ERROR(this->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 ;i<novelsNum;i++)
        {
            response->novels.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<SingleDogNode>("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服务通信客户端步骤

1. 导入服务接口
2. 创建请求结果接收回调函数
3. 声明并创建客户端
4. 编写结果接收逻辑
5. 调用客户端发送请求

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_interfaces</depend>

第二步修改和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<PoorManNode>("zhang3");
    /* 运行节点，并检测rclcpp状态*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

3.3.2 声明客户端

这里只是声明客户端，并指明这个客户端的类型是village_interfaces::srv::SellNovel

    // 创建一个客户端
    rclcpp::Client<village_interfaces::srv::SellNovel>::SharedPtr client_;

3.3.3 创建请求函数和回调函数

在class里的public下新建两个函数，第一个函数用于买小说，第二个函数用于回调买小说的结果response。

/*买小说函数*/
    void buy_novel()
    {
        RCLCPP_INFO(this->get_logger(), "买小说去喽");
        
    }
    
    /*接收小说-回调函数*/
    void novels_callback(rclcpp::Client<village_interfaces::srv::SellNovel>::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<village_interfaces::srv::SellNovel>::SharedFuture  response)
    {
    
    }
private:
    // 创建一个客户端
    rclcpp::Client<village_interfaces::srv::SellNovel>::SharedPtr client_;
};

/*主函数*/
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    /*产生一个PoorManNode的节点*/
    auto node = std::make_shared<PoorManNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

3.4 实例化客户端、编写请求函数和回调函数

3.4.1 实例化客户端

这个我们很熟悉了，先声明再实例化，在构造函数里加一句话。

//实例化客户端
client_ = this->create_client<village_interfaces::srv::SellNovel>("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<village_interfaces::srv::SellNovel_Request>();
        //先来五块钱的看看好不好看
        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<village_interfaces::srv::SellNovel_Request>();

创建好钱袋（request）之后，我们开始往钱袋里装钱，这里装了5块钱。

//先来五块钱的看看好不好看
request->money = 5; 

第三步发送请求

这一步我们和Python中一样，发送的是异步请求，并且设置了一个回调函数，意思是当我们请求（买书）成功时，请调用这个回调函数，把结果通过回调函数的参数传递过来。

关于回调函数前面提过好几次了，小鱼也在公众号上写了相关的文章，不明白的同学可以前往翻阅。

3.4.3 编写回调函数处理结果

接着编写回调函数novels_callback处理结果，完整的回调函数内容如下，也很简单，获取结果然后遍历结果打印。

注意函数的参数并不是SellNovel的Response对象，而是SharedFuture，这个小鱼会写篇文章说一说。这里直接用就行，使用response的get()获取。

    //创建接收到小说的回调函数
    void novels_callback(rclcpp::Client<village_interfaces::srv::SellNovel>::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<PoorManNode>();
    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+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小智，一起交流技术，学习机器人