pub_back_pressure.py

import rclpy

from rclpy.duration import Duration
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
from std_msgs.msg import String

rclpy.init()

node = rclpy.create_node("pub")

qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE, # Messages are transferred reliably (needs stateful writer)
    history=HistoryPolicy.KEEP_ALL,         # And we want to transfer all messages (since the reader has been made known to the writer and is alive) 
    depth=0,                                # doesn't matter
    durability=DurabilityPolicy.VOLATILE,   # We only provide messages created after a reader has been made known to the writer
    )
pub = node.create_publisher(String, "back_pressure", qos)

n = 0
def publish():
    global n

    # With back-pressure we have basically two possibilities here:
    #  1. block
    #  2. indicate that publishing right now is not possible (due to resource
    #     limit) by return value or exception
    # DDS allows for both.
    t1 = node.get_clock().now()
    try:
        # Use enormous messages to make sure that we'll soon crash if there are
        # unbounded queues at play
        pub.publish(String(data=str(n).ljust(512 * 1024,  " ")))
    except Exception as e:
        print("Failed to publish at", n, "with", e)
        return
    t2 = node.get_clock().now()

    # 20ms is plenty of time to queue a message
    if t2 - t1 > Duration(seconds=20e-3):
        print("Slowed down at", n, "for", f"{(t2 - t1).nanoseconds / 1e6:.3f} ms")

    n += 1
    if n % 10000 == 0:
        print(n)

timer = node.create_timer(0.001, publish)

while rclpy.ok():
    rclpy.spin(node)