Skip to content

Instantly share code, notes, and snippets.

@cleitner
Created March 30, 2020 14:20
Show Gist options
  • Save cleitner/e5eb35f6bcf6639425c06c03dd13fbff to your computer and use it in GitHub Desktop.
Save cleitner/e5eb35f6bcf6639425c06c03dd13fbff to your computer and use it in GitHub Desktop.
import rclpy
import time
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("sub")
slow_phase = True
start_time = time.time()
n = -1
def on_message(msg):
global n, slow_phase, start_time
received_n = int(msg.data)
if n != -1 and received_n > n + 1:
print("Lost a message! Shouldn't happen. Expected", n + 1, ", got", received_n)
n = received_n
# Slow subscriber takes its time
time.sleep(0.1)
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
)
sub = node.create_subscription(String, "back_pressure", on_message, qos)
while rclpy.ok():
rclpy.spin(node)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment