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

breaksoftware

于 2024-07-29 00:30:00 发布

阅读量425

点赞数 5

分类专栏： Robot Operating System 文章标签： c++ 机器人

本文链接：https://blog.csdn.net/breaksoftware/article/details/140661255

版权

Robot Operating System 专栏收录该内容

19 篇文章 0 订阅

订阅专栏

大纲

introspection_service
introspection_client
- 完整代码
测试
参考资料

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

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

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

本文我们将通过例子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_PUBLIC
  explicit 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.cpp
    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;
      };
      
    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.cpp
    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;
        }
      };
      
    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_event

namespace demo_nodes_cpp
{

class IntrospectionServiceNode : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit 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::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)

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_event

namespace demo_nodes_cpp
{
class IntrospectionClientNode : public rclcpp::Node
{
public:
  DEMO_NODES_CPP_PUBLIC
  explicit 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::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)

测试

在两个终端中，分别启动服务端和客户端
在这里插入图片描述

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

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

breaksoftware

关注

5
点赞
踩
7

收藏

觉得还不错? 一键收藏
打赏
0
评论
Robot Operating System——内部审查（Introspection）Service

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

扫一扫