ROS2通讯服务质量Qos介绍与样例

小鱼

ROS2通讯服务质量Qos介绍与样例

为了兼容不同场景下的通信需求，ROS 2 提供了服务多样的 Qos 服务质量来调整通信策略，但同时也带来了一些问题，比如明明话题发布正常，但是无法被订阅，RVIZ2 无法显示传感器消息等等，这些都是 Qos 的兼容性和配置造成的问题，本文主要来自 ROS2 中文文档，最后结合 FishBot 的里程计话题订阅和显示来介绍 Qos 在实际样例中的使用。

一、概述

ROS 2提供了丰富多样的服务质量（QoS）策略，允许您调整节点之间的通信。通过正确设置一组QoS策略，ROS 2可以像TCP一样可靠，也可以像UDP一样尽力而为，在其中有许多可能的状态。与ROS 1不同，ROS 2不仅主要支持TCP，还受益于底层DDS传输的灵活性，适用于有丢包的无线网络环境，其中“尽力而为 best effort”的策略更适合，或者在实时计算系统中需要满足截止时间的正确QoS配置文件。

一组QoS“策略”组合在一起形成一个QoS“配置文件”。鉴于为特定情景选择正确的QoS策略的复杂性，ROS 2提供了一组预定义的QoS配置文件，适用于常见用例（例如传感器数据）。同时，开发人员可以灵活控制QoS配置文件的特定策略。

可以为发布者、订阅者、服务服务器和客户端指定QoS配置文件。一个QoS配置文件可以独立应用于上述实体的每个实例，但如果使用不同的配置文件，可能会导致不兼容，从而阻止消息的传递。

二、QoS policies (策略):

当前的基本QoS配置文件包括以下策略设置：

History (历史):

Keep last (仅保留最新): 仅存储最多N个样本，可通过队列深度选项进行配置。
Keep all (全部保留): 存储所有样本，受底层中间件的资源限制配置的影响。
Depth (深度):
Queue size (队列大小): 仅在"history"策略设置为"keep last"时生效。

Reliability (可靠性):

Best effort (尽力而为): 尝试传递样本，但如果网络不稳定可能会丢失。
Reliable (可靠传递): 保证样本被传递，可能会多次重试。

Durability (持久性):

Transient local (瞬态本地): 发布者负责为"后续加入"的订阅保留样本。
Volatile (易失性): 不尝试保留样本。

Deadline (截止时间):

Duration (持续时间): 期望的两个连续消息发布到主题之间的最长时间。

Lifespan (寿命):

Duration (持续时间): 发布和接收消息之间的最长时间，而不会被视为陈旧或过期（过期消息会被悄悄丢弃，实际上永远不会被接收）。

Liveliness (活跃度):

Automatic (自动): 当节点的任何一个发布者发布消息时，系统将考虑所有发布者仍然存活另一个"租约持续时间"。
Manual by topic (按主题手动): 如果发布者通过调用发布者API手动声明自己仍然存活，系统将考虑发布者仍然存活另一个"租约持续时间"。

Lease Duration (租约持续时间):

Duration (持续时间): 发布者在系统认为其失去活跃性之前必须指示自己仍然存活的最长时间段（失去活跃性可能是故障的指示）。

对于每个不是持续时间的策略，还有"系统默认"选项，它使用底层中间件的默认设置。对于每个持续时间的策略，还存在一个"默认"选项，表示持续时间未指定，底层中间件通常会将其解释为一个无限长的持续时间。

打开RVIZ2,随便添加一个话题，在 Topic的下面就可以看到这些配置项目：

与ROS 1的比较

ROS 2中的“历史”和“深度”策略结合在一起，提供了类似于ROS 1中队列大小的功能。

ROS 2中的“可靠性”策略类似于在ROS 1中使用UDPROS（仅在roscpp中）以获得“尽力而为”，或使用TCPROS（ROS 1默认）以获得“可靠”。但请注意，即使在ROS 2中的可靠策略也是使用UDP实现的，这允许进行多播（如果合适的话）。

ROS 2中的“持久性”策略“瞬态本地”，与任何深度结合，提供了与“latching”发布者类似的功能。ROS 2中的其余策略与ROS 1中可用的任何内容都不相似，这意味着在这方面，ROS 2更具特色。未来可能会在ROS 2中提供更多的QoS策略。

三、Qos配置文件

配置文件允许开发人员专注于应用程序，而不必担心可能的每个QoS设置。QoS配置文件定义了一组预期在特定用例中很好配合的策略。

当前定义的QoS配置文件如下：

发布者和订阅者的默认QoS设置

为了使从ROS 1到ROS 2的过渡更加容易，希望实现类似的网络行为。默认情况下，ROS 2中的发布者和订阅者的"历史"采用"保留最后"，队列大小为10，"可靠性"采用"可靠"，"持久性"采用"volatile"，"活跃性"采用"系统默认"。截止日期、寿命和租约持续时间也都设置为"默认"。

服务

与发布者和订阅者类似，服务也具有可靠性。对于服务来说，使用“volatile”持久性尤为重要，否则重新启动的服务服务器可能会收到过时的请求。虽然客户端受到多个响应的保护，但服务器却没有受到接收过时请求的副作用的保护。

传感器数据

对于传感器数据来说，大多数情况下，及时接收读数比确保所有数据到达更为重要。也就是说，开发者希望尽快获取最新的采样数据，即使可能会丢失一些数据。因此，传感器数据配置文件使用尽力而为的可靠性和较小的队列大小。

参数

ROS 2 中的参数基于服务，因此具有类似的配置文件。不同之处在于参数使用更大的队列深度，这样当参数客户端无法连接参数服务服务器时，请求不会丢失。

系统默认值

这使用RMW实现的默认值来设置所有策略。不同的RMW实现可能具有不同的默认值。

点击此处https://github.com/ros2/rmw/blob/humble/rmw/include/rmw/qos_profiles.h 获取上述配置中使用的具体策略。这些配置文件中的设置可能会根据社区的反馈进行进一步调整。

四、QoS 兼容性

**注意：**本部分内容适用于发布者和订阅者，同样适用于服务端和客户端。

可以独立为发布者和订阅者配置 QoS 配置文件。仅当发布者和订阅者具有兼容的 QoS 配置文件时，才会建立发布者和订阅者之间的连接。

QoS配置文件的兼容性是基于"请求与提供"模型确定的。订阅者请求一个QoS配置文件，表示它愿意接受的"最低质量"，而发布者提供一个QoS配置文件，表示它能够提供的"最高质量"。只有在所请求的QoS配置文件的每个策略都不比提供的QoS配置文件更严格的情况下，才会建立连接。即使所请求的QoS配置文件不同，多个订阅者也可以同时连接到单个发布者。发布者和订阅者之间的兼容性不受其他发布者和订阅者的影响。

以下表格显示了不同策略设置的兼容性及其结果：

要实现"latched"主题，以便向后续订阅者可见，发布者和订阅者必须都同意使用"瞬态本地"。

为了建立连接，必须使所有影响兼容性的策略相互兼容。例如，即使请求和提供的QoS配置文件对可靠性QoS策略是兼容的，但如果它们的持久性QoS策略不兼容，仍然无法建立连接。

当无法建立连接时，发布者和订阅者之间将不会传递任何消息。有机制可以检测到这种情况，将在后面的章节中介绍。

与ROS 1的比较

在ROS 1中，具有相同消息类型和相同主题的任何发布者和订阅者都将建立连接。在使用ROS 2时，需要注意可能存在不兼容的请求和提供的QoS配置文件的情况。

五、QoS 事件

一些QoS策略与可能的事件相关联。开发人员可以为每个发布者和订阅者提供与这些QoS事件相关的回调函数，并以他们认为合适的方式处理这些事件，类似于处理在主题上接收到的消息的方式。

开发人员可以订阅与发布者相关联的以下QoS事件：

提供的截止期限未达成，发布者在截止期限QoS策略规定的预期持续时间内未发布消息。
活跃性丧失，发布者未在租约期限内指示其活跃性。
提供的QoS不兼容，发布者遇到了在相同主题上订阅的请求一个无法满足提供的QoS配置文件的QoS配置文件，导致发布者与该订阅之间没有连接。

开发者可以订阅与订阅相关的以下QoS事件：

请求的截止时间已过，该订阅在预期的持续时间内未收到消息，该持续时间由截止时间的QoS策略设置。
可用性发生变化，订阅发现，某个或多个发布者在订阅的主题上在租约期内未能表明其存活状态。
请求的QoS不兼容。，订阅遇到了同一主题上提供不符合请求的QoS配置文件的发布者，导致订阅与该发布者之间没有连接。

六、Qos 问题及使用样例

以下问题及解决方案来自社区：

我想要自己写一个节点来对fishbot小车发布的odom数据进行evo文件排版的储存，但是终端提示qos策略不对，/odom话题是best_effort。以下是此种情况的代码和日志：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class NodeToEvo(Node):
    def __init__(self,name):
        super().__init__(name)

        self.get_logger().info("transformation starts")
        self.subscribe = self.create_subscription(Odometry,"odom",self.callback,10)
        self.path = "/home/cxw"
        self.file = self.path + "/ground_truth_odom" + '.txt'
        self.writter = open(self.file,'w')


    def callback(self,msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')


def main(args=None):
    rclpy.init(args=args)
    node = NodeToEvo("ToEvo")
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

然后启动microros接收小车的数据

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

编译和source完后，ros2 run前面的py程序，日志报错如下：

cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo 
[INFO] [1691715773.163893894] [ToEvo]: transformation starts
[WARN] [1691715774.291494228] [ToEvo]: New publisher discovered on topic 'odom', offering incompatible QoS. No messages will be received from it. Last incompatible policy: RELIABILITY

然后查看小车odom的info：

cxw@pc:~/dev_ws$ ros2 topic info --verbose /odom 
Type: nav_msgs/msg/Odometry

Publisher count: 1

Node name: fishbot_motion_control
Node namespace: /
Topic type: nav_msgs/msg/Odometry
Endpoint type: PUBLISHER
GID: 01.0f.bc.e6.2d.00.ca.28.01.00.00.00.00.00.01.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 0

然后我去网上查，查到了一些在程序里修改qos策略的贴子，但是还是没有跑起来。修改后的代码:

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy

class NodeToEvo(Node):
    def __init__(self,name):
        super().__init__(name)

        qos_profile = QoSProfile(
			reliability = QoSReliabilityPolicy.BEST_EFFORT,
			history = QoSHistoryPolicy.UNKNOWN,
            depth = 10
		)

        self.get_logger().info("transformation starts")
        self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
        self.path = "/home/cxw"
        self.file = self.path + "/ground_truth_odom" + '.txt'
        self.writter = open(self.file,'w')


    def callback(self,msg):
        position = msg.pose.pose.position
        orientation = msg.pose.pose.orientation
        self.writter.write(position.x+'\t'+position.y+'\t'+position.z+'\t'+orientation.x+'\t'+orientation.y+'\t'+orientation.z+'\t'+orientation.w+'\n')


def main(args=None):
    rclpy.init(args=args)
    node = NodeToEvo("ToEvo")
    rclpy.spin(node)
    node.destroy_node() 
    rclpy.shutdown()

编译后运行的终端log：

cxw@pc:~/dev_ws$ ros2 run learning_topic toEvo 
[INFO] [1691716079.532903710] [ToEvo]: transformation starts

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'Unknown QoS history policy, at ./src/qos.cpp:61'

with this new error message:

  'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221'

rcutils_reset_error() should be called after error handling to avoid this.
<<<

>>> [rcutils|error_handling.c:108] rcutils_set_error_state()
This error state is being overwritten:

  'create_publisher() failed setting topic QoS, at ./src/subscription.cpp:221, at ./src/rcl/subscription.c:108'

with this new error message:

  'invalid allocator, at ./src/rcl/subscription.c:218'

rcutils_reset_error() should be called after error handling to avoid this.
<<<
invalid allocator, at ./src/rcl/subscription.c:218
Traceback (most recent call last):
  File "/home/cxw/dev_ws/install/learning_topic/lib/learning_topic/toEvo", line 33, in <module>
    sys.exit(load_entry_point('learning-topic==0.0.0', 'console_scripts', 'toEvo')())
  File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 34, in main
    node = NodeToEvo("ToEvo")
  File "/home/cxw/dev_ws/install/learning_topic/lib/python3.10/site-packages/learning_topic/toEvo.py", line 20, in __init__
    self.subscribe = self.create_subscription(Odometry,"odom",self.callback,qos_profile)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/node.py", line 1370, in create_subscription
    subscription_object = _rclpy.Subscription(
rclpy._rclpy_pybind11.RCLError: Failed to create subscription: invalid allocator, at ./src/rcl/subscription.c:218
[ros2run]: Process exited with failure 1

这下我就找不到解答这个的贴子了，求大佬指教

来自小鱼的回复：这样写

from nav_msgs.msg import  Odometry
self.subscription = self.create_subscription(
            Odometry,
            '/odom',
            self.listener_callback,
            rclpy.qos.qos_profile_sensor_data)

C++

odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>(
        "odom", rclcpp::SensorDataQoS(),