ROS2学习CH2 服务通信

ROS2的服务通信介绍

在CH1中，我们学习了ROS2的Topic通信机制，在通信时需要一个发布者(Publisher)和一个订阅者(Subscriber)，发布者将消息发布到Topic上，订阅者从Topic上接收消息。

显然，这种通信机制有着如下特点

• 单向异步通信：订阅者不需要阻塞等待发布者发布消息，发布者也不需要订阅者的响应
• 多对多通信：由于Topic是一个中间媒介，多个发布者可以向同一个Topic发布消息，多个订阅者也可以从同一个Topic接收消息
• 持续的数据流通信：Topic通信机制适合于持续的数据流通信，例如传感器数据的发布和接收。

但是有些场景中，通信并不是单一的持续传输数据，而是需要请求和响应的机制，例如机器人控制指令的发送和执行结果的返回，这种通信机制就需要使用服务通信。

服务(Service)通信在通信时有两个部分组成，一个是服务端(Server)，另一个是客户端(Client)。服务端提供服务，客户端请求服务。服务通信机制的特点如下：

• 双向同步通信：客户端发送请求(Request)后，需要等待服务端的响应(Response)，服务端接收到请求后，处理请求并返回响应给客户端。
• 一对一通信：每个服务端只能处理一个客户端的请求，客户端发送请求后，需要等待服务端的响应。

其通信流程如下图所示：

可以看出，服务通信通常适用于将数据发送后进行处理并返回处理结果的场景。要支持这种场景的实现，显然我们需要传递的消息接口(interface)需要包含请求(Request)和响应(Response)两部分的消息定义。因此，在服务通信的背景下，消息接口不再像普通的msg消息那样只包含一个消息定义，而是需要包含两个消息定义，一个是请求消息(Request Message)，另一个是响应消息(Response Message)。我们可以使用下面的指令查看当前ROS2中消息接口

ros2 interface list | grep srv

不难看出，服务通信的消息接口都放在package/srv目录下，消息接口的命名规则为：package_name/srv/ServiceName，例如std_srvs/srv/SetBool。我们可以使用下面的指令查看某个服务接口的定义

ros2 interface show std_srvs/srv/SetBool

可以看到，在这个接口中，比普通的msg接口多了一个---分隔符，分隔符前后各有不同的类型定义。这便是服务通信的请求(Request)和响应(Response)的定义。请求(Request)定义在分隔符前，响应(Response)定义在分隔符后。

# in package/srv/test.srv
Request Defination

--- # 分隔符

Response Defination

当客户端向服务端发送请求时，客户端会将请求消息(Request Message)发送给服务端，服务端接收到请求消息后，会根据请求消息的内容进行处理，并生成响应消息(Response Message)返回给客户端。客户端接收到响应消息后，就可以根据响应消息的内容进行相应的处理。

以最简单的turtlesim的服务通信为例，turtlesim提供了一个名为/spawn的服务接口，客户端可以向该服务发送请求消息，请求turtlesim在指定的位置生成一个新的乌龟，并返回生成的乌龟的名称作为响应消息。我们可以使用下面的指令查看该服务接口的定义

ros2 interface show turtlesim/srv/Spawn

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

可以看出，请求消息(Request Message)包含了乌龟的生成位置(x, y, theta)和名称(name)，响应消息(Response Message)包含了生成的乌龟的名称(name)。客户端发送请求消息后，服务端会根据请求消息的内容生成一个新的乌龟，并返回生成的乌龟的名称作为响应消息。

使用ros2 service call 服务名 服务接口类型 "请求消息"指令可以向服务端发送请求消息，并接收响应消息。例如，我们可以使用下面的指令向turtlesim的/spawn服务发送请求消息，请求在位置(2.0, 2.0)生成一个新的乌龟，并返回生成的乌龟的名称作为响应消息

ros2 run trutlesim turtlesim_node # 启动小海龟模拟器

# in new terminal 
ros2 service call /spawn turtlesim/srv/Spawn "{x: 5.0, y: 5.0, theta: 5.0, name: 'Rose'}"
# 注意Request格式为"{参数1: 值1, 参数2: 值2, ...}"，参数名必须与服务接口定义中的请求消息(Request Message)的参数名一致，参数值可以是任意合法的值。

执行后的结果如下

可以看出，客户端发送请求消息后，服务端接收到请求消息并在指定位置生成了一个新的乌龟，并返回生成的乌龟的名称作为响应消息。客户端接收到响应消息后，就可以根据响应消息的内容进行相应的处理。

自定义服务接口

类似于第一章中的自定义的消息接口(msg interface)，我们也可以使用ros提供的规范来定义自己的服务接口(srv interface)。自定义服务接口的步骤如下：

1. 在自己的package中创建srv目录，并在srv目录下创建一个srv文件，例如

mkdir -p my_package/srv
touch my_package/srv/MyService.srv
# 接口文件的命名规则为ServiceName.srv，ServiceName可以是任意合法的名称，建议使用大驼峰命名法(每个单词的首字母大写)

在srv文件中定义请求消息(Request Message)和响应消息(Response Message)，例如

# in div_interface/srv/MyService.srv
float32 a
float32 b
---
int8 SUCCESS=1
int8 FAIL=0
int8 result # 结果，1表示成功，0表示失败(比如b为0时，返回失败)
float32 div    # div = a/b

结束后需要在package.xml中添加对rosidl_default_generators的依赖，并在CMakeLists.txt中添加对srv文件的编译指令，例如

<!-- 在package.xml中添加下面的语句 -->

<member_of_group>rosidl_interface_packages</member_of_group> <!--表明该package包含ROS接口定义 -->
<depend>rosidl_default_generators</depend> <!-- 添加对rosidl_default_generators的依赖 -->

# 在CMakeLists.txt中添加下面的语句

find_package(rosidl_default_generators REQUIRED) # 添加对rosidl_default_generators的依赖
rosidl_generate_interfaces(${PROJECT_NAME}
  "srv/MyService.srv"
)                                                # 添加对srv文件的编译指令

构建(colcon build)以及source后，可以使用ros2 interface list | grep srv指令查看自定义的服务接口是否已经生成成功。

ros2 interface list | grep srv

可以看到，我们自定义的服务接口已经生成成功，然后使用ros2 interface show package_name/srv/MyService指令查看自定义的服务接口的定义

ros2 interface show div_interface/srv/MyService

我们自定义的服务接口的定义就显示了出来。至此，我们就完成了自定义服务接口的定义。

服务端实现

类似于话题通信需要节点实现发布者(Publisher)和订阅者(Subscriber)，服务通信也需要节点实现服务端(Server)和客户端(Client)。在ROS2中，服务端(Server)和客户端(Client)的实现方式与话题通信类似，都是通过创建节点(Node)来实现的。

一个简单的在节点中实现服务端(Server)的例子如下，它基于上面的自定义服务接口(div_interface/srv/MyService.srv)实现了一个简单的除法服务端(Server)，当客户端(Client)发送请求消息(Request Message)时，服务端(Server)会根据请求消息的内容进行处理，并返回响应消息(Response Message)给客户端(Client)。

// in div_server.cpp
// in pkg: div_service
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include "div_interface/srv/my_service.hpp"

using div_data = div_interface::srv::MyService;

class DivService : public rclcpp::Node
{   
    private:
        rclcpp::Service<div_interface::srv::MyService>::SharedPtr service_;
    public:
        DivService() : Node("div_service_node")
        {
            // 创建服务对象，并指定调用服务时的回调函数
            // 第一个参数是服务的名称，第二个参数是回调函数，回调函数的参数必须有所调用接口的Request和Response类型
            service_ = this->create_service<div_interface::srv::MyService>(
                "div", 
                [this]
                (const div_data::Request::SharedPtr request, div_data::Response::SharedPtr response)
                {
                    // 回调函数参数必须有所调用接口的Request和Response类型
                    this->div_service_callback(request, response);
                }
            );
        }

        void div_service_callback(div_data::Request::SharedPtr request, div_data::Response::SharedPtr response)
        {
            RCLCPP_INFO(this->get_logger(), "Received request: a=%f, b=%f", request->a, request->b);
            if(std::abs(request->b) < 1e-6)
            {
                // 处理除数为零的情况
                RCLCPP_ERROR(this->get_logger(), "Division by zero error!");
                response->result = div_data::Response::FAIL;
                response->div = 0;
            }
            else
            {
                response->div = request->a / request->b;
                response->result = div_data::Response::SUCCESS;
                RCLCPP_INFO(this->get_logger(), "Sending response: result=%d, div=%f", response->result, response->div);
            }
        }
};



int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<DivService>();

    rclcpp::spin(node);

    rclcpp::shutdown();
    return 0;
}

可以看到，在代码中我们使用this->create_service<服务接口类型>(服务名称, 回调函数)创建了一个服务对象，并指定了调用服务时的回调函数。在回调函数中，我们根据请求消息(Request Message)的内容进行处理，并生成响应消息(Response Message)返回给客户端(Client)。

其中，Request和Response是服务接口的两个嵌套类型，分别表示请求消息(Request Message)和响应消息(Response Message)。在回调函数中，我们可以通过request->参数名访问请求消息(Request Message)的参数，通过response->参数名访问响应消息(Response Message)的参数。且在作为参数时，必须使用SharedPtr类型，这是ROS2中对消息的内存管理方式，这使得我们在回调函数中可以直接使用request和response指针来访问请求消息(Request Message)和响应消息(Response Message)的参数，同时通过指针直接修改响应消息(Response Message)的参数值。

因此，创建一个简单的服务端(Server)节点的步骤如下：

1. 创建一个继承自rclcpp::Node的类，并在类的构造函数中使用this->create_service<服务接口类型>(服务名称, 回调函数)创建一个服务对象，并指定调用服务时的回调函数。
2. 在回调函数中，根据请求消息(Request Message)的内容进行处理，并生成响应消息(Response Message)返回给客户端(Client)。
3. 在main函数中，使用rclcpp::init初始化ROS2，创建服务端(Server)节点对象，并使用rclcpp::spin启动节点，等待客户端(Client)的请求消息(Request Message)。
4. 在回调函数中，使用request->参数名访问请求消息(Request Message)的参数，使用response->参数名访问响应消息(Response Message)的参数，并通过指针直接修改响应消息(Response Message)的参数值。

在使用colcon build以及source后，我们可以使用下面的指令启动服务端(Server)节点

# ros2 run pkg_name node_name
ros2 run div_service div_server_node

然后使用ros2 service list指令查看当前系统中所有的服务列表。

ros2 service list

在服务运行时，我们可以使用ros2 service call 服务名 服务接口类型 "请求消息"指令向服务端(Server)发送请求消息(Request Message)，并接收响应消息(Response Message)。例如，我们可以使用下面的指令向自定义的除法服务(div)发送请求消息，请求计算5.0除以2.0，并返回计算结果作为响应消息

注意，请求消息的格式为"{参数1: 值1, 参数2: 值2, ...}"，参数名必须与服务接口定义中的请求消息(Request Message)的参数名一致，参数值可以是任意合法的值。

ros2 service call /div div_interface/srv/MyService "{a: 5.0, b: 2.0}"

运行的结果如下图：

可以看到，当我们call服务时，服务端(Server)接收到请求消息(Request Message)后，计算了5.0除以2.0的结果，并返回了计算结果作为响应消息(Response Message)。

同时服务端的终端也会输出调用服务回调函数的日志信息，显示接收到的请求消息(Request Message)的参数值，以及发送的响应消息(Response Message)的参数值。

客户端实现

客户端的实现流程与服务端类似，服务端接受客户端发送的request然后返回response。而对于客户端，则需要发送request并等待服务端调用服务后返回的response。也就是说这个步骤中，客户端发送request与接受response之间有一段等待时间。因此对于这段时间的处理衍生出了两种不同的发送方式：同步调用 和异步调用。

以下是一个简单的在节点中实现客户端(Client)的例子，它基于上面的自定义服务接口(div_interface/srv/MyService.srv)实现了一个简单的除法客户端(Client)，当客户端(Client)发送请求消息(Request Message)时，客户端(Client)会等待服务端(Server)的响应消息(Response Message)，并根据响应消息(Response Message)的内容进行相应的处理。

// in div_client.cpp
// in pkg: div_service
#include "rclcpp/rclcpp.hpp"
#include "div_interface/srv/my_service.hpp"


class DivClient : public rclcpp::Node
{
    private:
        rclcpp::Client<div_interface::srv::MyService>::SharedPtr client_;

    public:
        DivClient() : Node("div_client_node")
        {
            // 创建客户端对象，并指定名称
            client_ = this->create_client<div_interface::srv::MyService>("div");
            // 注意create_client这个函数的参数为对应的服务名称，必须与服务端创建服务时的名称一致
            // 否则无法发送请求成功
            
        }

        // 发送除法请求, a,b：anwser = a/b
        void send_div_request(double a, double b)
        {
            // 等待服务端先上线。
            while(this->client_->wait_for_service(std::chrono::seconds(1)) == false)
            {
                if(!rclcpp::ok()) // 检查rclcpp是否正常运行（如按下Ctrl+C后）
                {
                    RCLCPP_ERROR(this->get_logger(), "rcl服务未启动，退出程序");
                    return;
                }
                RCLCPP_INFO(this->get_logger(), "等待服务上线...");
            }

            // 构造请求的对象
            auto request = std::make_shared<div_interface::srv::MyService::Request>();
            request->a = a;
            request->b = b;

            // 发送请求

            //1. 异步发送请求，使用回调函数处理响应
            #if 0
            this->client_->async_send_request(request, 
            [this](rclcpp::Client<div_interface::srv::MyService>::SharedFuture result_future)->void
            {
                auto response = result_future.get();
                if(response->result == div_interface::srv::MyService::Response::SUCCESS)
                {
                    RCLCPP_INFO(this->get_logger(), "除法成功，结果为: %f", response->div);
                }
                else
                {
                    RCLCPP_ERROR(this->get_logger(), "除法失败，除数不能为零");
                }
            }
            );
            #endif

            // 2. 同步发送请求，等待响应
            #if 1
            auto future = this->client_->async_send_request(request);
            // 等待响应
            rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
            // 处理响应
            auto response = future.get();
            if(response->result == div_interface::srv::MyService::Response::SUCCESS)
            {
                RCLCPP_INFO(this->get_logger(), "除法成功，结果为: %f", response->div);
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "除法失败，除数不能为零");
            }
            #endif
        }
};


int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<DivClient>();
    node->send_div_request(10.0, 2.0); // 发送除法请求，计算10.0除以2.0

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

在客户端节点的构造函数中仅做了客户端对象的创建。使用this->create_client<服务接口类型>(服务名称)创建了一个客户端对象，并指定了服务的名称。注意这里的服务名称一定要和客户端要调用的服务端的服务名称一致，否则客户端无法发送请求成功。

随后最重要的就是发送请求的函数send_div_request，在该函数中去完成一次发送请求的完整流程，具体流程如下

• 等待服务端上线
• 构造请求对象
• 发送请求(request)
• 处理响应(response)

在函数中我们使用这样一段代码去等待服务端上线

while(this->client_->wait_for_service(std::chrono::seconds(1)) == false)
{
    if(!rclcpp::ok()) // 检查rclcpp是否正常运行（如按下Ctrl+C后）
    {
        RCLCPP_ERROR(this->get_logger(), "rcl服务未启动，退出程序");
        return;
    }
    RCLCPP_INFO(this->get_logger(), "等待服务上线...");
}

其中this->client_->wait_for_service(std::chrono::seconds(1))是一个阻塞函数，它会在指定时间内(这里使用std::chrono::seconds(1)即1秒钟)等待服务端上线(也就是服务端的节点正在运行)，如果服务端在指定的时间内没有上线，则返回false。我们可以在while循环中不断调用该函数，直到服务端上线为止。

其中rclcpp::ok()是一个检查ROS2是否正常运行的函数，当外部按下Ctrl+C或者节点程序异常退出时，rclcpp::ok()会返回false，我们可以在while循环中检查该函数的返回值，如果返回false，则说明ROS2已经停止运行，我们可以在while循环中退出程序。若没有这一句，则当服务端未上线时，客户端会一直等待，无法退出程序。

当服务端上线后，我们就可以构造请求对象，并发送请求了。我们可以使用std::make_shared<服务接口类型::Request>()创建一个请求对象，并设置请求对象的参数值。此时我们可以使用两种方式发送请求，一种是异步发送请求，另一种是同步发送请求。异步发送请求时，我们可以使用回调函数处理响应，而同步发送请求时，我们需要等待响应的返回。

同步发送对应的代码如下

 // 2. 同步发送请求，等待响应
    #if 1
    auto future = this->client_->async_send_request(request);
    // 等待响应
    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
    // 处理响应
    auto response = future.get();
    if(response->result == div_interface::srv::MyService::Response::SUCCESS)
    {
        RCLCPP_INFO(this->get_logger(), "除法成功，结果为: %f", response->div);
    }
    else
    {
        RCLCPP_ERROR(this->get_logger(), "除法失败，除数不能为零");
    }
    #endif

在发送时，我们使用this->client_->async_send_request(request)发送请求，该函数会返回一个类型rclcpp::Client<div_interface::srv::MyService>::FutureAndRequestId的对象。使用其get()接口可以获取到响应对象。但是当刚刚发送请求后，服务端可能还没有处理完请求并返回响应，因此我们需要等待响应的返回，此时的future.get()返回的响应才有意义。

因此这里使用rclcpp::spin_until_future_complete(this->get_node_base_interface(), future)等待响应的返回。该函数会阻塞当前线程，直到响应返回或者超时。当阻塞结束后，我们就可以使用future.get()获取响应对象，并根据响应对象的内容进行相应的处理。

这种方法使用spin_until_future_complete阻塞等待响应的返回，实现了客户端的同步调用。另一种方法是使用回调函数处理响应，实现客户端的异步调用。异步调用的代码如下

 //1. 异步发送请求，使用回调函数处理响应
    #if 0
    this->client_->async_send_request(request, 
    [this](rclcpp::Client<div_interface::srv::MyService>::SharedFuture result_future)->void
    {
        auto response = result_future.get();
        if(response->result == div_interface::srv::MyService::Response::SUCCESS)
        {
            RCLCPP_INFO(this->get_logger(), "除法成功，结果为: %f", response->div);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "除法失败，除数不能为零");
        }
    }
    );
    #endif

可以看到整个发送流程仅用了一个函数，即 this->client_->async_send_request(request, 回调函数)，显然这是async_send_request函数的重载版本。该函数的第一个参数是请求对象，第二个参数是回调函数。当服务端处理完请求并返回响应时，该回调函数会被调用，并将响应对象作为参数传递给回调函数。在回调函数中，我们可以使用result_future.get()获取响应对象，并根据响应对象的内容进行相应的处理。

这种方法实现了客户端的异步调用，客户端发送请求后，不会阻塞等待响应的返回，而是继续执行后续的代码。当服务端处理完请求并返回响应时，回调函数会被调用，并将响应对象作为参数传递给回调函数。在回调函数中，我们可以根据响应对象的内容进行相应的处理。

总结，创建一个简单的客户端(Client)节点的步骤如下：

1. 创建一个继承自rclcpp::Node的类，并在类的构造函数中使用this->create_client<服务接口类型>(服务名称)创建一个客户端对象，并指定服务的名称。
2. 在类中定义一个发送请求的函数
3. 在发送请求的函数中，使用this->client_->wait_for_service(std::chrono::seconds(1))等待服务端上线，使用std::make_shared<服务接口类型::Request>()创建一个请求对象，并设置请求对象的参数值。
4. 使用同步/异步的方式发送请求，处理响应。

最后，在main函数中，使用rclcpp::init初始化ROS2，创建客户端(Client)节点对象，并调用发送请求的函数发送请求消息(Request Message)，然后使用rclcpp::spin启动节点，等待服务端(Server)的响应消息(Response Message)。

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<DivClient>();
    node->send_div_request(10.0, 2.0); // 发送除法请求，计算10.0除以2.0

    rclcpp::spin(node);

    rclcpp::shutdown();

    return 0;
}

编译并分别运行服务端和客户端节点

# in terminal 1
ros2 run div_service div_server_node
# in terminal 2
ros2 run div_service div_client_node

运行的结果如下所示

可以看到当先启动客户端节点，后启动服务端节点时，客户端节点会一直等待服务端上线，直到服务端上线后，客户端节点才会发送请求消息(Request Message)给服务端，并接收响应消息(Response Message)，并打印相关的日志信息。