ROS2高效学习第四章 -- ros2 topic 编程之测试 DDS 的 QoS 其三

1 前言与资料

在ROS2高效学习第一章 – ros2整体介绍及DDS引入中，我们系统介绍了 ros2 引入 DDS（Data Distribution Service，数据分发服务）的前因后果。作为 ros2 topic 编程第三篇，本文仍将复用第一篇ROS2高效学习第四章 – ros2 topic 编程之收发 string 并使用 ros2 launch创建的两个软件包，即 pubsub_cpp 和 pubsub_py，在此基础上开发 QoS（Quality of Service，服务质量） 样例，实际测试 ros2 DDS 的功能。
本文参考资料如下：
（1）ROS2高效学习第一章 – ros2整体介绍及DDS引入
（2）古月 ros2 DDS

2 正文

2.1 DDS QoS 引入

（1）QoS 含义：Quality of Service，服务质量，这是通信领域一个非常重要的概念，用于控制发布者和订阅者之间的通信策略。只看他的名字很难直接理解他的含义，但可以把 QoS 映射为几个对应的配置进行直观理解。请执行下面的命令，查看 QoS 默认配置。

# 随便发一个 topic
ros2 topic pub /chatter std_msgs/msg/Int32 "data: 42"
# 新开一个窗口，查看这个 topic 的默认 QoS profile
ros2 topic info /chatter --verbose

（2）QoS 配置逐项解析：

Reliability：消息传达的可靠性；
	RELIABLE(default): 保证传输所有消息，丢失的消息会重传，类似 TCP 。
	BEST_EFFORT: 尽最大努力传送消息，但不保证交付，不会重传丢失的消息，类似 UDP。
补充：机器人系统中，传感器数据通常使用BEST_EFFORT，而参数更新需要RELIABLE可靠性。

History：消息的历史记录方式，当订阅者由于忙碌错过了消息，其仍能收到历史消息。这个参数只有在 Reliability 为 RELIABLE 时才有用。
	KEEP_LAST(default): 缓冲区保留最后几条消息。
	KEEP_ALL: 缓冲区保留所有传递过的消息。
Depth：与 History 搭配使用，当 History 为 KEEP_LAST 时，指定需要保留的消息个数，默认是10个；

Durability：消息是否应该在网络中持久化，以便新加入的订阅者也能接收。这个参数只有在 Reliability 为 RELIABLE 时才有用
	TRANSIENT_LOCAL(default): 消息在发布者端持久化，新加入的订阅者可以接收到旧消息，接收个数由History和Depth决定。
	VOLATILE: 消息不在节点之间持久化，新加入的订阅者接收不到旧消息。

Lifespan：消息应该在缓存中保留多长时间。如果超过这个时间，消息会被丢弃，即使还没有被任何订阅者接收，默认值是 Infinite。
	提示：Lifespan 的设置要与 History 和 Depth 对应起来，两个配置遵循木桶原理，最短的起作用。

Deadline：确定消息应当被发布和接收的最大期限，超出后会执行特定的回调函数，默认值是 Infinite。这个参数一般用于实时系统。
	举例：deadline=Duration(seconds=2, nanoseconds=0)，

Liveliness: 节点如何通告自己的活动状态，用来确定节点是否还活着。一般用于分布式系统
    AUTOMATIC(default): 节点自动报告其存活状态。
    MANUAL_BY_TOPIC: 发布者需要手动报告其针对特定话题的存活状态。
Liveliness lease duration: 与 Liveliness 搭配使用，表示节点或话题必须在此期限内报告其存活状态，否则认为已经不活跃，默认值是 Infinite。

（3）发布者和订阅者 QoS 配置要求：针对 topic 配置 QoS ，一般要求同一个 topic 收发两端的 QoS 配置要一致，不然就会有奇奇怪怪的问题。
（4）命令行测试 QoS 的方法：ros2 topic 命令行有一组qos选项，可以手动测试 QoS 。

# 使用 best_effort 发送 /chatter
ros2 topic pub /chatter std_msgs/msg/Int32 "data: 42" --qos-reliability best_effort
# 使用 reliable 订阅 /chatter，这样是收不到的！
ros2 topic echo /chatter  --qos-reliability reliable
# 使用 best_effort 订阅 /chatter，欧凯！
ros2 topic echo /chatter --qos-reliability best_effort

2.2 pubsub_py – 测试新加入的节点能收到历史消息

（1）创建新的 python 和 launch 文件

cd ~/colcon_ws/src/pubsub_py
touch launch/pubsub_qos_launch.py
touch pubsub_py/pub_hello_qos.py pubsub_py/sub_hello_qos.py

（2）编写 pub_hello_qos.py

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

import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy, LivelinessPolicy
from rclpy.duration import Duration

class Publisher(Node):
    def __init__(self):
        super().__init__('test_qos_publisher')

        qos_profile = QoSProfile(
            # RELIABLE(default), BEST_EFFORT
            reliability = QoSReliabilityPolicy.RELIABLE,
            # VOLATILE(default)
            # TRANSIENT_LOCAL(Only works when reliability is RELIABLE and needs history:KEEP_LAST and depth)
            # 新加入的节点能收到历史消息，这是最重要的配置！！！！！
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            # history: only works when reliability is RELIABLE
            # KEEP_LAST(default), KEEP_ALL
            history = QoSHistoryPolicy.KEEP_LAST,
            # default is 10
            depth=10,
            # default is Infinite
            lifespan=Duration(seconds=5),

            # default is Infinite
            deadline=Duration(seconds=5, nanoseconds=0),
            # AUTOMATIC(default), MANUAL_BY_TOPIC
            liveliness=LivelinessPolicy.AUTOMATIC,
            # default is Infinite
            liveliness_lease_duration=Duration(seconds=5)
        )

        self._publisher = self.create_publisher(String, "hello_topic", qos_profile)
        self._timer = self.create_timer(0.5, self.timer_callback)
        self._i = 0

    def timer_callback(self):
        msg = String()
        msg.data = "hello, i am fine in python! %d" % self._i
        self._publisher.publish(msg)
        self.get_logger().info("publish: %s" % msg.data)
        self._i += 1

def main(args=None):
    rclpy.init(args=args)
    pub_node = Publisher()
    rclpy.spin(pub_node)
    pub_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

（3）编写 sub_hello_qos.py

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

import rclpy
from rclpy.node import Node

from std_msgs.msg import String
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy, LivelinessPolicy
from rclpy.duration import Duration
from time import sleep

class Subscriber(Node):
    def __init__(self):
        super().__init__('test_qos_subscriber')

        qos_profile = QoSProfile(
            # RELIABLE(default), BEST_EFFORT
            reliability = QoSReliabilityPolicy.RELIABLE,
            # VOLATILE(default)
            # TRANSIENT_LOCAL(Only works when reliability is RELIABLE and needs history:KEEP_LAST and depth)
            # 新加入的节点能收到历史消息，这是最重要的配置！！！！！
            durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
            # history: only works when reliability is RELIABLE
            # KEEP_LAST(default), KEEP_ALL
            history = QoSHistoryPolicy.KEEP_LAST,
            # default is 10
            depth=10,
            # default is Infinite
            lifespan=Duration(seconds=5),

            # default is Infinite
            deadline=Duration(seconds=5, nanoseconds=0),
            # AUTOMATIC(default), MANUAL_BY_TOPIC
            liveliness=LivelinessPolicy.AUTOMATIC,
            # default is Infinite
            liveliness_lease_duration=Duration(seconds=5)
        )

        self._subscriber = self.create_subscription(String, "hello_topic", self.topic_callback, qos_profile)

    def topic_callback(self, msg):
        self.get_logger().info("subscribe: %s" % msg.data)
        # sleep(2)        # test qos history and depth parameters

def main(args=None):
    rclpy.init(args=args)
    sub_node = Subscriber()
    rclpy.spin(sub_node)
    sub_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

（4）编写 pubsub_qos_launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import TimerAction

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='pubsub_py',
            namespace='qos',
            executable='talker_qos',
            name='talker_qos'
        ),
        TimerAction(
            period=3.0,   # test qos durability:TRANSIENT_LOCAL
            # period=0.0,
            actions=[
                Node(
                    package='pubsub_py',
                    namespace='qos',
                    executable='listener_qos',
                    name='listener_qos'
                )
            ]
        )
    ])

（5）添加 setup.py

    entry_points={
        'console_scripts': [
            'talker = pubsub_py.publisher:main',
            'talker_diy = pubsub_py.pub_diy_msg:main',
            'listener = pubsub_py.subscriber:main',
            'listener_diy = pubsub_py.sub_diy_msg:main',
            'talker_qos = pubsub_py.pub_hello_qos:main',
            'listener_qos = pubsub_py.sub_hello_qos:main'
        ],
    },

（6）编译并运行

~/colcon_ws
colcon build --packages-select diy_interface pubsub_py
source install/local_setup.bash
ros2 launch pubsub_py pubsub_qos_launch.py

2.3 pubsub_cpp – 测试订阅者能接收错过的消息

代码链接：pubsub_cpp，这里不再详述，基本就是用 cpp 实现上面的 python 版本。不同的是，cpp 样例重点测试：订阅者因忙碌错过了一些消息，但通过 QoS 仍能接收。

3 总结

本文的代码托管在本人的github上：pubsub_py，pubsub_cpp