【ROS2】演示:配置服务自省

目标：为服务客户端和服务器配置服务内省。

 教程级别：高级

 时间：15 分钟

 目录

•  概述

•  安装演示

• 内省配置状态

•  内省演示

•  相关内容

 概述 

ROS 2 应用程序通常由服务组成，以在远程节点中执行特定程序。可以通过服务内省来检查服务数据通信。

在此演示中，我们将重点介绍如何为服务客户端和服务器配置服务自省状态，并监控与 ros2 service echo 的服务通信。

安装演示 

请参阅安装说明以了解安装 ROS 2 的详细信息。

如果您已经安装了 ROS 2 二进制包，请确保已安装 ros-jazzy-demo-nodes-cpp 。如果您下载了存档或从源代码构建了 ROS 2，它将已经是安装的一部分。

内省配置状态 

服务自省有 3 种配置状态。

服务自省配置状态 

RCL_SERVICE_INTROSPECTION_OFF	禁用
RCL_SERVICE_INTROSPECTION_METADATA	仅元数据，没有任何用户数据内容
RCL_SERVICE_INTROSPECTION_CONTENTS	用户数据内容与元数据

 内省演示 

此演示展示了如何使用 ros2 service echo 管理服务自省和监控服务数据通信。

内省服务节点:

https://github.com/ros2/demos/blob/jazzy/demo_nodes_cpp/src/services/introspection_service.cpp

#include <cinttypes> // 包含固定宽度整数类型的头文件
#include <memory> // 包含智能指针库
#include <vector> // 包含向量容器库


#include "rcl/service_introspection.h" // 包含服务内省的头文件


#include "rclcpp/qos.hpp" // 包含质量服务（QoS）设置的头文件
#include "rclcpp/rclcpp.hpp" // 包含ROS 2的核心库
#include "rclcpp_components/register_node_macro.hpp" // 包含注册节点宏的头文件


#include "example_interfaces/srv/add_two_ints.hpp" // 包含示例服务AddTwoInts的头文件
#include "rcl_interfaces/msg/set_parameters_result.hpp" // 包含设置参数结果消息的头文件


#include "demo_nodes_cpp/visibility_control.h" // 包含可见性控制的头文件


// 这个演示程序展示了如何通过参数动态配置服务内省。
// 该程序由一个服务节点（IntrospectionServiceNode）组成，该节点监听'/add_two_ints'服务的客户端请求。
// 当客户端连接并发送请求时，它会将两个整数相加并返回结果。
//
// 上述是一个相当常见的ROS 2服务，但该程序试图展示的是内省功能。
// IntrospectionServiceNode有一个名为'service_configure_introspection'的字符串参数。
// 如果该参数设置为'disabled'（默认值），则不会进行内省。
// 如果该参数设置为'metadata'（参见下面的参数设置详情），则会将基本元数据（时间戳、序列号等）发送到一个隐藏的主题/add_two_ints/_service_event。
//
// 要查看此功能，请运行以下命令：
//
// ros2 launch demo_nodes_cpp introspect_services_launch.py
// 由于内省的默认设置是'disabled'，这与普通的客户端和服务器没有区别。
// 不会创建额外的主题，也不会发送内省数据。然而，动态更改内省配置是完全支持的。
// 这可以通过运行'ros2 param set /introspection_service service_configure_introspection metadata'来实现，
// 这将配置服务开始将内省元数据发送到/add_two_ints/_service_event。
//
// 一旦设置了参数，可以通过运行以下命令查看内省数据：
// ros2 topic echo /add_two_ints/_service_event


namespace demo_nodes_cpp // 定义命名空间demo_nodes_cpp
{


class IntrospectionServiceNode : public rclcpp::Node // 定义IntrospectionServiceNode类，继承自rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit IntrospectionServiceNode(const rclcpp::NodeOptions & options) // 构造函数，接受节点选项作为参数
  : Node("introspection_service", options) // 调用基类构造函数，设置节点名称为"introspection_service"
  {
    auto handle_add_two_ints = [this](
      const std::shared_ptr<rmw_request_id_t> request_header,
      const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request> request,
      std::shared_ptr<example_interfaces::srv::AddTwoInts::Response> response) -> void // 响应参数
      {
        (void)request_header; // 忽略请求头
        RCLCPP_INFO( // 打印信息日志
          this->get_logger(), "Incoming request\na: %" PRId64 " b: %" PRId64,
          request->a, request->b);
        response->sum = request->a + request->b; // 计算并设置响应结果
      };
    // 创建一个服务，使用回调函数处理请求
    srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);


    auto on_set_parameter_callback = // 定义设置参数的回调函数
       [](std::vector<rclcpp::Parameter> parameters) {
        rcl_interfaces::msg::SetParametersResult result; // 创建设置参数结果消息
        result.successful = true; // 设置结果为成功
        for (const rclcpp::Parameter & param : parameters) { // 遍历参数
          if (param.get_name() != "service_configure_introspection") { // 如果参数名称不是'service_configure_introspection'
            continue; // 跳过
          }


          if (param.get_type() != rclcpp::ParameterType::PARAMETER_STRING) { // 如果参数类型不是字符串
            result.successful = false; // 设置结果为失败
            result.reason = "must be a string"; // 设置失败原因
            break; // 退出循环
          }


          if (param.as_string() != "disabled" && param.as_string() != "metadata" &&
            param.as_string() != "contents") // 如果参数值不是'disabled'、'metadata'或'contents'
          {
            result.successful = false; // 设置结果为失败
            result.reason = "must be one of 'disabled', 'metadata', or 'contents'"; // 设置失败原因
            break; // 退出循环
          }
        }


        return result; // 返回结果
      };


    auto post_set_parameter_callback = // 定义设置参数后的回调函数
      [this](const std::vector<rclcpp::Parameter> & parameters)  {
        for (const rclcpp::Parameter & param : parameters) { // 遍历参数
          if (param.get_name() != "service_configure_introspection") { // 如果参数名称不是'service_configure_introspection'
            continue; // 跳过
          }


          rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; // 定义内省状态，默认关闭


          if (param.as_string() == "disabled") { // 如果参数值为'disabled'
            introspection_state = RCL_SERVICE_INTROSPECTION_OFF; // 设置内省状态为关闭
          } else if (param.as_string() == "metadata") { // 如果参数值为'metadata'
            introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; // 设置内省状态为元数据
          } else if (param.as_string() == "contents") { // 如果参数值为'contents'
            introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; // 设置内省状态为内容
          }


          this->srv_->configure_introspection( // 配置服务内省
            this->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
          break; // 退出循环
        }
      };


    on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback( // 添加设置参数的回调函数
      on_set_parameter_callback);
    post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback( // 添加设置参数后的回调函数
      post_set_parameter_callback);


    this->declare_parameter("service_configure_introspection", "disabled"); // 声明参数'service_configure_introspection'，默认值为'disabled'
  }


private:
  rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_; // 服务指针
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
    on_set_parameters_callback_handle_; // 设置参数的回调函数句柄
  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr
    post_set_parameters_callback_handle_; // 设置参数后的回调函数句柄
};


}  // namespace demo_nodes_cpp // 命名空间结束


RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionServiceNode) // 注册节点

默认情况下禁用服务自省，因此用户需要启用它才能在服务服务器上调用 configure_introspection 。在此演示中， IntrospectionServiceNode 使用名为 `service_configure_introspection 的参数来配置服务自省状态。

首先我们需要开始 IntrospectionServiceNode 。

$ ros2 run demo_nodes_cpp introspection_service

要更改服务自省状态，我们需要按如下方式设置 configure_introspection 参数。

### User data contents with metadata
$ ros2 param set /introspection_service service_configure_introspection contents
### Or only metadata
$ ros2 param set /introspection_service service_configure_introspection metadata
### To disable
$ ros2 param set /introspection_service service_configure_introspection disabled

cxy@cxy-Ubuntu2404:~$ ros2 param set /introspection_service service_configure_introspection contents
Set parameter successful

内省客户端节点:

https://github.com/ros2/demos/blob/jazzy/demo_nodes_cpp/src/services/introspection_client.cpp

#include <chrono> // 包含时间库
#include <memory> // 包含智能指针库
#include <vector> // 包含向量容器库


#include "rcl/service_introspection.h" // 包含服务内省的头文件


#include "rclcpp/qos.hpp" // 包含质量服务（QoS）设置的头文件
#include "rclcpp/rclcpp.hpp" // 包含ROS 2的核心库
#include "rclcpp_components/register_node_macro.hpp" // 包含注册节点宏的头文件


#include "example_interfaces/srv/add_two_ints.hpp" // 包含示例服务AddTwoInts的头文件
#include "rcl_interfaces/msg/set_parameters_result.hpp" // 包含设置参数结果消息的头文件


#include "demo_nodes_cpp/visibility_control.h" // 包含可见性控制的头文件


// 这个演示程序展示了如何通过参数动态配置客户端内省。
// 该程序由一个客户端节点（IntrospectionClientNode）组成，该节点有一个每500毫秒运行一次的定时器回调。
// 如果服务尚未准备好，则不进行进一步工作。
// 如果客户端当前没有正在进行的请求，则创建一个新的AddTwoInts服务请求，并异步发送给服务。
// 当该请求完成时，它将标志设置为没有请求在进行中，以便发送另一个请求。
//
// 上述是一个相当常见的ROS 2客户端，但该程序试图展示的是内省功能。
// IntrospectionClientNode有一个名为'client_configure_introspection'的字符串参数。
// 如果该参数设置为'disabled'（默认值），则不会进行内省。
// 如果该参数设置为'metadata'（参见下面的参数设置详情），则会将基本元数据（时间戳、序列号等）发送到一个隐藏的主题/add_two_ints/_service_event。
//
// 要查看此功能，请运行以下命令：
//
// ros2 launch demo_nodes_cpp introspect_services_launch.py
// 由于内省的默认设置是'disabled'，这与普通的客户端和服务器没有区别。
// 不会创建额外的主题，也不会发送内省数据。然而，动态更改内省配置是完全支持的。
// 这可以通过运行'ros2 param set /introspection_client client_configure_introspection metadata'来实现，
// 这将配置客户端开始将内省元数据发送到/add_two_ints/_service_event。
//
// 一旦设置了参数，可以通过运行以下命令查看内省数据：
// ros2 topic echo /add_two_ints/_service_event


namespace demo_nodes_cpp // 定义命名空间demo_nodes_cpp
{
class IntrospectionClientNode : public rclcpp::Node // 定义IntrospectionClientNode类，继承自rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit IntrospectionClientNode(const rclcpp::NodeOptions & options) // 构造函数，接受节点选项作为参数
  : Node("introspection_client", options) // 调用基类构造函数，设置节点名称为"introspection_client"
  {
    client_ = create_client<example_interfaces::srv::AddTwoInts>("add_two_ints"); // 创建AddTwoInts服务客户端


    auto on_set_parameter_callback = // 定义设置参数的回调函数
       [](std::vector<rclcpp::Parameter> parameters) {
        rcl_interfaces::msg::SetParametersResult result; // 创建设置参数结果消息
        result.successful = true; // 设置结果为成功
        for (const rclcpp::Parameter & param : parameters) { // 遍历参数
          if (param.get_name() != "client_configure_introspection") { // 如果参数名称不是'client_configure_introspection'
            continue; // 跳过
          }


          if (param.get_type() != rclcpp::ParameterType::PARAMETER_STRING) { // 如果参数类型不是字符串
            result.successful = false; // 设置结果为失败
            result.reason = "must be a string"; // 设置失败原因
            break; // 退出循环
          }


          if (param.as_string() != "disabled" && param.as_string() != "metadata" &&
            param.as_string() != "contents") // 如果参数值不是'disabled'、'metadata'或'contents'
          {
            result.successful = false; // 设置结果为失败
            result.reason = "must be one of 'disabled', 'metadata', or 'contents'"; // 设置失败原因
            break; // 退出循环
          }
        }


        return result; // 返回结果
      };


    auto post_set_parameter_callback = // 定义设置参数后的回调函数
      [this](const std::vector<rclcpp::Parameter> & parameters) {
        for (const rclcpp::Parameter & param : parameters) { // 遍历参数
          if (param.get_name() != "client_configure_introspection") { // 如果参数名称不是'client_configure_introspection'
            continue; // 跳过
          }


          rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF; // 定义内省状态，默认关闭


          if (param.as_string() == "disabled") { // 如果参数值为'disabled'
            introspection_state = RCL_SERVICE_INTROSPECTION_OFF; // 设置内省状态为关闭
          } else if (param.as_string() == "metadata") { // 如果参数值为'metadata'
            introspection_state = RCL_SERVICE_INTROSPECTION_METADATA; // 设置内省状态为元数据
          } else if (param.as_string() == "contents") { // 如果参数值为'contents'
            introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; // 设置内省状态为内容
          }


          this->client_->configure_introspection( // 配置客户端内省
            this->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);
          break; // 退出循环
        }
      };


    on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback( // 添加设置参数的回调函数
      on_set_parameter_callback);
    post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback( // 添加设置参数后的回调函数
      post_set_parameter_callback);


    this->declare_parameter("client_configure_introspection", "disabled"); // 声明参数'client_configure_introspection'，默认值为'disabled'


    timer_ = this->create_wall_timer( // 创建定时器，每500毫秒运行一次
      std::chrono::milliseconds(500),
      [this]() {
        if (!client_->service_is_ready()) { // 如果服务尚未准备好
          return; // 返回
        }


        if (!request_in_progress_) { // 如果没有正在进行的请求
          auto request = std::make_shared<example_interfaces::srv::AddTwoInts::Request>(); // 创建AddTwoInts请求
          request->a = 2; // 设置请求参数a
          request->b = 3; // 设置请求参数b
          request_in_progress_ = true; // 设置请求进行中标志
          client_->async_send_request( // 异步发送请求
            request,
            [this](rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedFuture cb_f) // 请求完成回调
            {
              request_in_progress_ = false; // 重置请求进行中标志
              RCLCPP_INFO(get_logger(), "Result of add_two_ints: %ld", cb_f.get()->sum); // 打印请求结果
            }
          );
          return; // 返回
        }
      });
  }


private:
  rclcpp::Client<example_interfaces::srv::AddTwoInts>::SharedPtr client_; // 客户端指针
  rclcpp::TimerBase::SharedPtr timer_; // 定时器指针
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
    on_set_parameters_callback_handle_; // 设置参数的回调函数句柄
  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr
    post_set_parameters_callback_handle_; // 设置参数后的回调函数句柄
  bool request_in_progress_{false}; // 请求进行中标志
};


}  // namespace demo_nodes_cpp // 命名空间结束


RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionClientNode) // 注册节点

然后，我们以相同的方式启动和配置 IntrospectionClientNode 。

ros2 run demo_nodes_cpp introspection_client

更改服务自省状态以设置 configure_introspection 参数如下。

### User data contents with metadata
$ ros2 param set /introspection_client client_configure_introspection contents
### Or only metadata
$ ros2 param set /introspection_client client_configure_introspection metadata
### To disable
$ ros2 param set /introspection_client client_configure_introspection disabled

在本教程中，以下是服务自省状态 CONTENTS 在 IntrospectionServiceNode 和 METADATA 在 IntrospectionClientNode 上的示例输出。要监控 IntrospectionClientNode 和 IntrospectionServiceNode 之间的服务通信，让我们运行它：

$ ros2 service echo --flow-style /add_two_ints
info:
  event_type: REQUEST_SENT
  stamp:
    sec: 1709432402
    nanosec: 680094264
  client_gid: [1, 15, 0, 18, 86, 208, 115, 86, 0, 0, 0, 0, 0, 0, 21, 3]
  sequence_number: 247
request: []
response: []
---
info:
  event_type: REQUEST_RECEIVED
  stamp:
    sec: 1709432402
    nanosec: 680459568
  client_gid: [1, 15, 0, 18, 86, 208, 115, 86, 0, 0, 0, 0, 0, 0, 20, 4]
  sequence_number: 247
request: [{a: 2, b: 3}]
response: []
---
info:
  event_type: RESPONSE_SENT
  stamp:
    sec: 1709432402
    nanosec: 680765280
  client_gid: [1, 15, 0, 18, 86, 208, 115, 86, 0, 0, 0, 0, 0, 0, 20, 4]
  sequence_number: 247
request: []
response: [{sum: 5}]
---
info:
  event_type: RESPONSE_RECEIVED
  stamp:
    sec: 1709432402
    nanosec: 681027998
  client_gid: [1, 15, 0, 18, 86, 208, 115, 86, 0, 0, 0, 0, 0, 0, 21, 3]
  sequence_number: 247
request: []
response: []
---
...

您可以看到 event_type: REQUEST_SENT 和 event_type: RESPONSE_RECEIVED ，这些自省服务事件发生在 IntrospectionClientNode 。这些事件不包括 request 和 response 字段中的任何内容，这是因为 IntrospectionClientNode 的服务自省状态设置为 METADATA 。另一方面，来自 IntrospectionServiceNode 的 event_type: REQUEST_RECEIVED 和 event_type: RESPONSE_SENT 事件包括 request: [{a: 2, b: 3}] 和 response: [{sum: 5}] ，因为自省状态设置为 CONTENTS 。

 相关内容 

• 服务自省客户端示例 (rclcpp) 和服务自省服务示例 (rclcpp).

• 服务自省客户端和服务示例 (rclpy). https://github.com/ros2/demos/blob/jazzy/demo_nodes_py/demo_nodes_py/services/introspection.py

• 服务内省 REP-2012. https://github.com/ros-infrastructure/rep/pull/360