Ros2 qos自定义策略实现方式（c c++ python）

Ros2 qos简介

用于节点之前的数据通信，为了保证数据能够可靠和高校的传输。ROS2提供了一套非常丰富的服务质量(Quality of Service, QoS)策略

Ros2 qos 注意事项

发布器、订阅器、服务端和客户端可以指定Profile，且它们的每个实例都可以单独指定Profile，但不兼容的Profiles可能导致无法通信。

Qos策略

当前，QoS Profile提供了对以下QoS策略的设置：

(1)历史记录(History)

保留近期记录(Keep last)：缓存最多N条记录，可通过队列长度选项来配置。

保留所有记录(Keep all)：缓存所有记录，但受限于底层中间件可配置的最大资源。

(2)深度(Depth)

队列深度(Size of the queue)：只能与Keep last配合使用。

(3)可靠性(Reliability)

尽力的(Best effort)：尝试传输数据但不保证成功传输(当网络不稳定时可能丢失数据)。

可靠的(Reliable)：反复重传以保证数据成功传输。

(4)持续性(Durability)

局部瞬态(Transient local)：发布器为晚连接(late-joining)的订阅器保留数据。

易变态(Volatile)：不保留任何数据。

以上每个策略都有系统默认值。这个默认值就是底层中间件的默认值，由DDS供应商工具(如XML配置文件)定义。DDS本身提供了许多可配置的策略。这些策略与ROS1的特征相似，所以在ROS1中是可见的。之后可能会有更多的策略在ROS2中可见。

自定义Ros2 qos 策略

• c版

• 首先创建一个default_qos.h,添加自定义的策略

#ifndef RCL_ACTION__DEFAULT_QOS_H_
#define RCL_ACTION__DEFAULT_QOS_H_

#ifdef __cplusplus
extern "C"
{
#endif

#include "rmw/types.h"


static const rmw_qos_profile_t rcl_msg_qos_profile_status_default =
{
  RMW_QOS_POLICY_HISTORY_KEEP_LAST,
  10,
  RMW_QOS_POLICY_RELIABILITY_RELIABLE,
  RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL,
  RMW_QOS_DEADLINE_DEFAULT,
  RMW_QOS_LIFESPAN_DEFAULT,
  RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT,
  RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT,
  false
};

#ifdef __cplusplus
}
#endif

#endif

• 第二步把定义好策略赋值到qos的结构体中

int start_msg(rcl_node_t* node, rclc_support_t* support)
{
  rcl_node_options_t node_ops = rcl_node_get_default_options();
  RCCHECK(rcl_node_init(node, "char_long_sequence_subscriber_rcl", "", &support->context, &node_ops))
  rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options();
  rcl_subscription_t subscription = rcl_get_zero_initialized_subscription();
  //设置配置好的qos策略
  subscription_ops.qos = rcl_msg_qos_profile_status_default;
  RCCHECK(rcl_subscription_init(&subscription, node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), "string data", &subscription_ops))
  rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
  RCCHECK(rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, &support->context, rcl_get_default_allocator()))
  std_msgs__msg__String msg;
  msg.data.data = (char * ) malloc(ARRAY_LEN * sizeof(char));
  msg.data.size = 0;
  msg.data.capacity = ARRAY_LEN;
  char test_array[ARRAY_LEN];
  memset(test_array,'z',ARRAY_LEN);
  do {
    RCSOFTCHECK(rcl_wait_set_clear(&wait_set))
    size_t index;
    RCSOFTCHECK(rcl_wait_set_add_subscription(&wait_set, &subscription, &index))
    RCSOFTCHECK(rcl_wait(&wait_set, RCL_MS_TO_NS(1)))
    if (wait_set.subscriptions[index]) {
      rcl_ret_t rc = rcl_take(wait_set.subscriptions[index], &msg, NULL, NULL);
      if (RCL_RET_OK == rc) {
        
        // Check if sequence items matches the know pattern
        bool pass_test = strncmp(test_array, msg.data.data, msg.data.size) == 0;

        printf("I received an %ld array. Test: [%s]\n", msg.data.size, (pass_test) ? "OK" : "FAIL");
      }
    }
  } while ( true );
  RCCHECK(rcl_subscription_fini(&subscription, node))
  RCCHECK(rcl_node_fini(node))
  return 0;
}

• python 版自定义qos策略添加

import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy
from rclpy.qos import QoSProfile
from rclpy.qos_event import SubscriptionEventCallbacks

from std_msgs.msg import String


class MinimalSubscriber(Node):

    def __init__(self):
        #创建qos对象
        qos = QoSProfile(depth=10)
        #设置不同的qos策略
        qos.durability = DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL

        custom_callback = lambda event: print("Custom Incompatible Callback!")
        callbacks = SubscriptionEventCallbacks()
        callbacks.incompatible_qos = custom_callback

        super().__init__('minimal_subscriber')
        #qos策略传入
        self.subscription = self.create_subscription(
            String,
            'string data',
            self.listener_callback,
            qos,
            event_callbacks=callbacks)

    def listener_callback(self, msg):
        self.get_logger().info('I heard: "%s"' % msg.data)


def main(args=None):
    rclpy.init(args=args)

    minimal_subscriber = MinimalSubscriber()

    rclpy.spin(minimal_subscriber)

    minimal_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

• c++版 自定义Qos策略添加

#include <iostream>
#include <functional>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

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

  auto node = std::make_shared<rclcpp::Node>("qos_callback_listener");
  rclcpp::SubscriptionOptions options;

  rclcpp::QoS qos(0);
  qos.reliable();
  qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);

  options.use_default_callbacks = false;
  options.event_callbacks.incompatible_qos_callback = [](rclcpp::QOSOfferedIncompatibleQoSInfo & event) {
    std::cout << "Custom Incompatible Callback!" << std::endl;
  };

  auto subscriber = node->create_subscription<std_msgs::msg::String>(
    "/string data",
    qos,
    [](const std_msgs::msg::String::SharedPtr) {
      std::cout << "Heard a message" << std::endl;
    },
    options);

  rclcpp::spin(node);
  return 0;
}