Robot Operating System——内部审查（Introspection）Service

在ROS 2（Robot Operating System 2）中，内部审查（Introspection）是一种强大的功能，它允许用户和开发者查询和理解系统中的实时状态和配置。这种能力特别重要，因为它提供了一种方法来监视和调试复杂的机器人系统，从而确保系统的健康和性能。

Introspection功能通常包括但不限于以下几个方面：

1. 节点状态和信息查询：允许查询系统中活动节点的状态，包括它们的名称、命名空间、正在运行的进程ID等信息。
2. 主题和服务发现：提供机制来发现系统中活动的主题和服务，包括它们的类型、发布者和订阅者。
3. 参数配置和管理：允许动态查询和设置节点参数，这对于调整系统行为和性能参数至关重要。
4. 服务调用和响应监控：能够监视服务请求和响应，帮助开发者理解系统中的交互和潜在问题。

本文我们将通过例子demo_nodes_cpp/src/services/introspection_service.cpp和demo_nodes_cpp/src/services/introspection_client.cpp来讲解如何使用内部审查（Introspection）功能。

introspection_service

demo_nodes_cpp/src/services/introspection_service.cpp沿用了《Robot Operating System——Service的同步/异步通信》的相关功能，提供了计算两个整型值之和的功能。本文我们不分析这块代码，只是贴出来供大家参考。

class IntrospectionServiceNode : public rclcpp::Node
{
public:DEMO_NODES_CPP_PUBLICexplicit IntrospectionServiceNode(const rclcpp::NodeOptions & options): Node("introspection_service", options){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;};// Create a service that will use the callback function to handle requests.srv_ = create_service<example_interfaces::srv::AddTwoInts>("add_two_ints", handle_add_two_ints);

和内部审查（Introspection）功能相关是之后的代码。

我们会通过Parameter参数service_configure_introspection来控制Node的内部审查（Introspection）功能的开启后者关闭。这种设计非常有必要，因为内部审查（Introspection）功能会消耗很多资源。如果我们不排查问题，最好将其关闭。

我们在后面的代码中可以发现，不管是Service还是Client，都默认将service_configure_introspection设置为disabled。

    this->declare_parameter("service_configure_introspection", "disabled");

检验Parameter值和类型

我们使用Parameter校验回调检测Parameter参数是否符合我们的设计。在本例中，我们要求自定义的service_configure_introspection是String类型，且其值只能是：“disabled”、“metadata"和"contents”。

对于校验回调函数相关知识可以参考《Robot Operating System——Parameter设置的预处理、校验和成功回调》。

// demo_nodes_cpp/src/services/introspection_service.cppauto 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") {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"){result.successful = false;result.reason = "must be one of 'disabled', 'metadata', or 'contents'";break;}}return result;};on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(on_set_parameter_callback);

修改内部审查（Introspection）功能的状态

如果上述校验通过，Parameter参数就会被设置成功。然后我们就可以在“成功回调”中通过Node::configure_introspection方法修改部审查（Introspection）功能的状态。

// demo_nodes_cpp/src/services/introspection_service.cppauto post_set_parameter_callback =[this](const std::vector<rclcpp::Parameter> & parameters) {for (const rclcpp::Parameter & param : parameters) {if (param.get_name() != "service_configure_introspection") {continue;}rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;if (param.as_string() == "disabled") {introspection_state = RCL_SERVICE_INTROSPECTION_OFF;} else if (param.as_string() == "metadata") {introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;} else if (param.as_string() == "contents") {introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS;}this->srv_->configure_introspection(this->get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state);break;}};post_set_parameters_callback_handle_ = this->add_post_set_parameters_callback(post_set_parameter_callback);

在ROS 2中，RCL_SERVICE_INTROSPECTION_OFF、RCL_SERVICE_INTROSPECTION_METADATA、和RCL_SERVICE_INTROSPECTION_CONTENTS是用于配置服务内部审查（Introspection）级别的枚举值。这些枚举值允许开发者控制ROS 2节点中服务的内省细节级别，以适应不同的监控和调试需求。

• RCL_SERVICE_INTROSPECTION_OFF:
  这个枚举值用于完全关闭服务的内省功能。当选择这个级别时，系统不会收集或提供任何关于服务调用的内省数据。这可以用于生产环境中，当性能是关键考虑因素，且不需要内省数据时。

• RCL_SERVICE_INTROSPECTION_METADATA:
  使用这个枚举值，服务的内省将仅包括基本的元数据信息，如服务的名称和类型。这提供了一种中等级别的内省，允许开发者和系统管理员了解服务的基本情况，而不会暴露服务调用的具体内容。这种级别的内省对于理解系统的结构和服务的布局很有用，同时保持了较高的隐私和性能。

• RCL_SERVICE_INTROSPECTION_CONTENTS:
  当设置为这个枚举值时，服务的内省将包括服务调用的完整内容，包括请求和响应数据。这是最详细的内省级别，允许开发者深入了解服务的运行情况和交互数据。这对于调试复杂的问题和性能分析特别有价值，但可能会增加系统的开销，并可能暴露敏感信息。

完整代码

// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.#include <cinttypes>
#include <memory>
#include <vector>#include "rcl/service_introspection.h"#include "rclcpp/qos.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"#include "example_interfaces/srv/add_two_ints.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"#include "demo_nodes_cpp/visibility_control.h"// This demo program shows how to configure service introspection on the fly,
// by hooking it up to a parameter.  This program consists of a service
// node (IntrospectionServiceNode) that listens on the '/add_two_ints' service
// for clients.  When one connects and sends a request, it adds the two integers
// and returns the result.
//
// The above is a fairly common ROS 2 service, but what this program is trying
// to demonstrate is introspection capabilities.  The IntrospectionServiceNode
// has a string parameter called 'service_configure_introspection'.  If this is
// set to 'disabled' (the default), then no introspection happens.  If this is set
// to 'metadata' (see details on how to set the parameters below), then
// essential metadata (timestamps, sequence numbers, etc) is sent to a hidden
// topic called /add_two_ints/_service_event.
//
// To see this in action, run the following:
//
// ros2 launch demo_nodes_cpp introspect_services_launch.py
//   Since the default for introspection is 'disabled', this is no different than
//   a normal client and server.  No additional topics will be made, and
//   no introspection data will be sent.  However, changing the introspection
//   configuration dynamically is fully supported.  This can be seen by
//   running 'ros2 param set /introspection_service service_configure_introspection metadata'
//   which will configure the service to start sending service introspection
//   metadata to /add_two_ints/_service_event.
//
// Once the parameter is set, introspection data can be seen by running:
//   ros2 topic echo /add_two_ints/_service_eventnamespace demo_nodes_cpp
{class IntrospectionServiceNode : public rclcpp::Node
{
public:DEMO_NODES_CPP_PUBLICexplicit IntrospectionServiceNode(const rclcpp::NodeOptions & options): Node("introspection_service", options){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;};// Create a service that will use the callback function to handle requests.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") {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"){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") {continue;}rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;if (param.as_string() == "disabled") {introspection_state = RCL_SERVICE_INTROSPECTION_OFF;} else if (param.as_string() == "metadata") {introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;} else if (param.as_string() == "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");}private:rclcpp::Service<example_interfaces::srv::AddTwoInts>::SharedPtr srv_;rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtron_set_parameters_callback_handle_;rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtrpost_set_parameters_callback_handle_;
};}  // namespace demo_nodes_cppRCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionServiceNode)

introspection_client

demo_nodes_cpp/src/services/introspection_client.cpp沿用了《Robot Operating System——Service的同步/异步通信》的中Client端相关功能，它会每隔500毫秒向Server端发送一个计算2+3之和的请求。

    timer_ = this->create_wall_timer(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>();request->a = 2;request->b = 3;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;}});

对于内部审查（Introspection）功能的开启关闭，它的逻辑和Service端一模一样，我们即不赘述了。

完整代码

// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.#include <chrono>
#include <memory>
#include <vector>#include "rcl/service_introspection.h"#include "rclcpp/qos.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"#include "example_interfaces/srv/add_two_ints.hpp"
#include "rcl_interfaces/msg/set_parameters_result.hpp"#include "demo_nodes_cpp/visibility_control.h"// This demo program shows how to configure client introspection on the fly,
// by hooking it up to a parameter.  This program consists of a client
// node (IntrospectionClientNode) that has a timer callback that runs every
// 500 milliseconds.  If the service is not yet ready, no further work is done.
// If the client doesn't currently have a request in flight, then it creates a
// new AddTwoInts service request, and asynchronously sends it to the service.
// When that request completes, it sets the flag back to having no requests in
// flight so another request is sent.
//
// The above is a fairly common ROS 2 client, but what this program is trying
// to demonstrate is introspection capabilities.  The IntrospectionClientNode
// has a string parameter called 'client_configure_introspection'.  If this is
// set to 'disabled' (the default), then no introspection happens.  If this is set
// to 'metadata' (see details on how to set the parameters below), then
// essential metadata (timestamps, sequence numbers, etc) is sent to a hidden
// topic called /add_two_ints/_service_event.
//
// To see this in action, run the following:
//
// ros2 launch demo_nodes_cpp introspect_services_launch.py
//   Since the default for introspection is 'disabled', this is no different than
//   a normal client and server.  No additional topics will be made, and
//   no introspection data will be sent.  However, changing the introspection
//   configuration dynamically is fully supported.  This can be seen by
//   running 'ros2 param set /introspection_client client_configure_introspection metadata'
//   which will configure the client to start sending service introspection
//   metadata to /add_two_ints/_service_event.
//
// Once the parameter is set, introspection data can be seen by running:
//   ros2 topic echo /add_two_ints/_service_eventnamespace demo_nodes_cpp
{
class IntrospectionClientNode : public rclcpp::Node
{
public:DEMO_NODES_CPP_PUBLICexplicit IntrospectionClientNode(const rclcpp::NodeOptions & options): Node("introspection_client", options){client_ = create_client<example_interfaces::srv::AddTwoInts>("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() != "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"){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") {continue;}rcl_service_introspection_state_t introspection_state = RCL_SERVICE_INTROSPECTION_OFF;if (param.as_string() == "disabled") {introspection_state = RCL_SERVICE_INTROSPECTION_OFF;} else if (param.as_string() == "metadata") {introspection_state = RCL_SERVICE_INTROSPECTION_METADATA;} else if (param.as_string() == "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");timer_ = this->create_wall_timer(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>();request->a = 2;request->b = 3;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::SharedPtron_set_parameters_callback_handle_;rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtrpost_set_parameters_callback_handle_;bool request_in_progress_{false};
};}  // namespace demo_nodes_cppRCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::IntrospectionClientNode)

测试

在两个终端中，分别启动服务端和客户端


然后再开一个终端查看内部审查数据

ros2 service echo --flow-style /add_two_ints

可以看到服务端和客户端在内部审查（Introspection）功能处于关闭状态时什么也看不到。

我们再开一个终端，修改服务端的Parameter。

ros2 param set /introspection_service service_configure_introspection metadata

这个时候我们就可以在审查终端数据开始更新。下图表示服务端收到一条客户端的请求，并发送返回给客户端。

我们将内部审查（Introspection）功能开启到RCL_SERVICE_INTROSPECTION_CONTENTS级别，然后观察变化。

ros2 param set /introspection_service service_configure_introspection contents

这次我们看到请求和返回都有了详细数据。

我们还可以对客户端开启内部审查（Introspection）功能，同样开启到RCL_SERVICE_INTROSPECTION_CONTENTS级别。

ros2 param set /introspection_client client_configure_introspection contents

可以看到审查终端中，数据变化的更快。一次请求被拆分成4个部门，分别是：

• 客户端的REQUEST_SENT。表示向服务端发起请求。
• 服务端的REQUEST_RECEIVED。表示服务端收到请求。
• 服务端的RESPONSE_SENT。表示服务端发送返回结果。
• 客户端的RESPONSE_RECEIVED。表示客户端收到返回结果。

参考资料

• https://docs.ros.org/en/iron/Tutorials/Demos/Service-Introspection.html