ROS2——手把手编写一个服务

ROS2——手把手教你编写一个服务

服务简介

上一篇文章讲的话题通信是基于订阅/发布机制的，无论有没有订阅者，发布者都会周期发布数据，这种模式适合持续数据的收发，比如传感器数据。机器人系统中还有另外一些配置性质的数据，并不需要周期处理，此时就要用到另外一种ROS通信方式——服务（Service）。服务是基于客户端/服务器模型的通信机制，服务器端只在接收到客户端请求时才会提供反馈数据。

服务有着以下特点:

• 同一个服务（名称相同）有且只能有一个节点来提供

  

• 同一个服务可以被多个客户端调用(可以一对一也可一对多)

  

下面, 我们将从一个实例出发, 手把手写一段服务程序

案例描述

来看一个案例: (以下均使用-er表示名词, 虽然没有这个词)

• 有一些穷人Poorer和一个发放免费汉堡的爱心机构Organization.
• Organization总共有一定数量的汉堡.
• Poorer可以为家人代领汉堡, 家里有n个人, 就可以领取2n个汉堡.
• 当然, 还有一些有钱人Richer想蹭吃蹭喝, Organization不会给这些人发放食物
• 当汉堡发完了, Organization发出通知, 不再发放.

以上案例实际上就实现了一个服务, Organization为服务端, Poorer和Richer为客户端

自定义服务接口

上一篇文章我们自定义了话题接口, 服务与话题不同, 服务是双向的, 所以要定义来回两个数据类型

编辑.srv文件

cd到工作空间的/src文件夹下, 新建接口包

cd ros2_ws/src
ros2 pkg create service_interfaces --build-type ament_cmake 

cd进入service_interfaces文件夹, 新建Organization.srv文件(首字母要求大写)

mkdir srv
touch srv/Organization.srv

编辑Organization.srv

来回两个数据中间用---分隔开

# 是穷人还是富人
string status
# 家里的人数
uint32 num_of_people
---
# 是否发出成功
bool success
# 发出的汉堡数量
uint32 num_of_hamburger

修改Cmakelist.txt

注意:rosidl_generate_interfaces()必须在 ament_package()前

# 添加依赖
find_package(rosidl_default_generators REQUIRED)

# 声明srv文件所属的工程名字, 文件位置, 依赖DEPENDENCIES
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/Calculate.srv"
  DEPENDENCIES 
 )

修改package.xml

<!-- 添加如下内容 -->
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

编译并刷新环境变量

colcon build --packages-select service_interfaces
source ~/.bashrc

详细查看接口

要想详细查看接口, 可以使用ros2 interface命令, 前面的博客已经讲过, 不再赘述

服务程序实现

下面来编写这段程序

新建工作空间

mkdir -p ros2_ws/src
cd ros2_ws/src

mkdir -p: 递归创建目录,即使上级目录不存在,会按目录层级自动创建目录

新建功能包

ros2 pkg create poor_and_organization --build-type ament_cmake --dependencies rclcpp service_interfaces

使用ament_cmake作为编译类型, 并使用依赖rclcpp和前文定义的service_interfaces接口包

在ros2_ws/src/poor_and_organization/src下创建Poor.cpp和Organization.cpp

编写服务端节点

直接献出程序, 每句都有注释, 看不懂你打我

#include "rclcpp/rclcpp.hpp"
#include "service_interfaces/srv/calculate.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

//创建一个类节点，名字叫做Organization,继承自Node.
class Organization : public rclcpp::Node 
{

public:
    // 初始化汉堡总数NumOfAll为100
    Organization() : Node("Organization"), NumOfAll(100)
    {
        // 热心组织的自我介绍
        RCLCPP_INFO(this->get_logger(), "大家好, 我们是热心组织, 我们只给poorer发汉堡.");
        // 实例化回调组, 作用为避免死锁(请自行百度ROS2死锁)
        callback_group_organization = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        // 实例化发汉堡的的服务
        Organization_Server = this->create_service<service_interfaces::srv::Calculate>("Calculate",
                                    std::bind(&Organization::organization_callback,this,_1,_2),
                                    rmw_qos_profile_services_default,
                                    callback_group_organization);
    }
private:
    // 剩余汉堡总数
    size_t NumOfAll;

    // 声明一个服务回调组
    rclcpp::CallbackGroup::SharedPtr callback_group_organization;

    // 声明一个服务端
    rclcpp::Service<service_interfaces::srv::Calculate>::SharedPtr Organization_Server;

    // 声明一个回调函数，当收到要汉堡请求时调用该函数
    void organization_callback(const service_interfaces::srv::Calculate::Request::SharedPtr request,
                               const service_interfaces::srv::Calculate::Response::SharedPtr response)
    {
        // 首先判断是不是穷人
        if(request->status == "Poorer")
        {
            // 打印Poorer家的人数
            RCLCPP_INFO(this->get_logger(), "收到一个来自%s的请求，他家有%d个人.", request->status.c_str(), request->num_of_people);
            // 计算应给汉堡数量，每人两个
            unsigned int NumOfRequired = request->num_of_people*2;
            // 检查剩余汉堡够不够分
            if(NumOfRequired > NumOfAll)
            {
                // 不够分了, 返回false
                RCLCPP_INFO(this->get_logger(), "当前汉堡库里只剩%d个汉堡啦! 已经不够分了, 请明日再来.", NumOfRequired);
                response->success = false;
            }
            else
            {
                // 够分, 更新剩余汉堡数量
                NumOfAll -= NumOfRequired;
                // 返回给几个汉堡
                response->num_of_hamburger = NumOfRequired;
                // 成功送出, 返回true
                response->success = true;
                RCLCPP_INFO(this->get_logger(), "成功送出%d个汉堡~ 还剩余%d个汉堡", NumOfRequired, NumOfAll);
            }
        }
        else
        {
            // 富人不给汉堡, 同样返回false, 同时返回送出汉堡数量为0
            response->success = false;
            response->num_of_hamburger = 0;
            RCLCPP_INFO(this->get_logger(), "收到一个非法请求，这人是个%s, 不满足送汉堡资格.", request->status.c_str());
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Organization>();
    // 把节点的执行器变成多线程执行器, 避免死锁
    rclcpp::executors::MultiThreadedExecutor exector;
    exector.add_node(node);
    exector.spin();
    rclcpp::shutdown();
    return 0;
}

编写客户端Poor节点

#include "rclcpp/rclcpp.hpp"
#include "service_interfaces/srv/calculate.hpp"

using namespace std::chrono_literals;

using std::placeholders::_1;

//创建一个类节点，名字叫做Poorer,继承自Node.
class Poorer : public rclcpp::Node
{
public:
    // 构造函数
    Poorer() : Node("Poorer")
    {
        // 打印一句自我介绍
        RCLCPP_INFO(this->get_logger(), "我是一个来领汉堡的人.");
        // 实例化客户端, 指明客户端的接口类型，同时指定要请求的服务的名称Calculate.
        Poorer_Client = this->create_client<service_interfaces::srv::Calculate>("Calculate");
    }

    int take_hamburger(int argc, char **argv)
    {
        RCLCPP_INFO(this->get_logger(), "现在去领取汉堡");
        
        //构造请求
        auto request = std::make_shared<service_interfaces::srv::Calculate::Request>();
        
        //等待服务端上线
        while (!Poorer_Client->wait_for_service(1s))
        {
            //等待时检测rclcpp的状态
            if (!rclcpp::ok())
            {
                // 检测到Ctrl+C直接退出
                RCLCPP_ERROR(this->get_logger(), "等待被打断, 不等了");
                rclcpp::shutdown();
                return 1;
            }
            // 否则一直等
            RCLCPP_INFO(this->get_logger(), "等待热心组织开门");
        }

        // 输入参数格式错误的时候报错并退出程序
        if (argc != 3)
        {
            RCLCPP_ERROR(this->get_logger(), "输入格式错误, 格式为: 什么人 几口人. 例如: ros2 run poor_and_organization Poor_node Poorer 3");
            rclcpp::shutdown();
            return 1;
        }
        else
        {
            // 格式正确, 获取参数, 放入request中
            request->status =             argv[1];
            request->num_of_people = atoi(argv[2]);
            RCLCPP_INFO(this->get_logger(), "我是%s, 我家有%d个人", request->status.c_str(), request->num_of_people);
        }

        //发送异步请求，然后等待返回，返回时调用回调函数
        Poorer_Client->async_send_request(request, std::bind(&Poorer::poorer_callback, this, _1));

        return 0;
    }
private:
    // 创建一个客户端
    rclcpp::Client<service_interfaces::srv::Calculate>::SharedPtr Poorer_Client;

    // 创建接收到小说的回调函数
    void poorer_callback(rclcpp::Client<service_interfaces::srv::Calculate>::SharedFuture response)
    {
        // 使用response的get()获取
        auto result = response.get();
        // 如果确实是Poorer, 则领取成功
        if(result->success == true)
        {
            RCLCPP_INFO(this->get_logger(), "成功领取%d个汉堡", result->num_of_hamburger);
            rclcpp::shutdown();
        }
        // 不是Poorer或者汉堡数量不够, 则领取失败
        else
        {
            RCLCPP_INFO(this->get_logger(), "领取汉堡失败, 原因可能为: 1.你不是Poorer 2.汉堡不够了");
            rclcpp::shutdown();
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    // 产生一个Poorer的节点
    auto node = std::make_shared<Poorer>();
    node->take_hamburger(argc, argv);
    // 运行节点，并检测rclcpp状态
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

Cmakelist.txt

find_package(rclcpp REQUIRED)
find_package(service_interfaces REQUIRED)

add_executable(Poor_node src/Poor.cpp)
ament_target_dependencies(Poor_node rclcpp service_interfaces)

add_executable(Organization_node src/Organization.cpp)
ament_target_dependencies(Organization_node rclcpp service_interfaces)

将编译好的文件安装到install/poor_and_organization/lib/poor_and_organization下

install(TARGETS
  Poor_node
  Organization_node
  DESTINATION lib/${PROJECT_NAME}
)

package.xml

同样地, 新建功能包的时候没有加--dependencies rclcpp service_interfaces等功能包, 则需要手动添加, 放置于<package>标签下

<depend>rclcpp</depend>
<depend>service_interfaces</depend>

也可自行修改下面这些声明, 与实现功能无关, 但是最好写全

<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="fanziqi@fanziqi.site">fanziqi</maintainer>
<license>TODO: License declaration</license>

编译

--packages-select指定编译customer_and_kfc功能包

colcon build --packages-select poor_and_organization

刷新环境

前文应该已经echo了环境, 如果没有, 请添加echo "source /ros2_ws/install/setup.zsh" >> ~/.bashrc

source ~/.bashrc

运行

新建一个终端窗口, 运行Organization服务端节点

ros2 run poor_and_organization Organization_node

再另新建一个终端, 运行Poor客户端节点

第一个参数为节点名称Poor_node, 第二个参数为人员类型Poorer, 第三个参数为家里人数5

ros2 run poor_and_organization Poor_node Poorer 5

此时应该可以看见:

Organization服务端: 成功发出10个汉堡

Poor客户端: 成功领取10个汉堡

也可以手动发布客户端一下

ros2 service call /Calculate service_interfaces/srv/Calculate "{num_of_people: 40, status: 'Poorer'}"

Poor客户端:

Organization服务端:

此时如果再申请20个汉堡, 会发生什么呢?

ros2 run poor_and_organization Poor_node Poorer 10

Poor客户端: 提示领取失败

Organization服务端: 提示不够分了

这时候有一个Richer来骗汉堡吃

ros2 run poor_and_organization Poor_node Richer 1

Poor客户端: 提示领取失败

Organization服务端: 就算有汉堡也不分给Richer

这时, Organization关门下班了(Ctrl+C关闭Organization_Node节点)

又来了一个人想要领汉堡:

ros2 run poor_and_organization Poor_node Poorer 2

Poor客户端: 提示等待开门, 不想等待的话直接按Ctrl+C退出

经验证, 需求全部实现~

服务常用命令

使用 ros2 service

查看服务列表

ros2 service list

具体点

ros2 service list -t

查看服务的接口

ros2 service type <service_name>

查看接口的服务

ros2 service find <type_name>

查看服务接口类型

ros2 interface show ***/srv/***

手动调用服务

前文使用过, 还记得吗

ros2 service call <service_name> <service_type> <arguments>