3232
3333import rclpy
3434from rclpy .node import Node
35+ from rclpy .qos import QoSHistoryPolicy , QoSReliabilityPolicy , QoSDurabilityPolicy , QoSLivelinessPolicy
3536from std_msgs .msg import Empty
3637
3738#define constants
@@ -56,29 +57,21 @@ def __init__(self) -> None:
5657
5758 #configure custom QoS profile for subscriber to handle timing issues and register
5859 #events when heartbeat not achieved
59- #set history to keep last so depth parameter is used (should be default)
60- #depth 1 as only need latest message
61- qos_profile = rclpy .qos .QoSProfile (history = rclpy .qos .QoSHistoryPolicy .KEEP_LAST ,
62- depth = 1 )
60+ #https://docs.ros.org/en/ros2_documentation/eloquent/Concepts/About-Quality-of-Service-Settings.html
61+ qos_profile = \
62+ rclpy .qos .QoSProfile (history = QoSHistoryPolicy .KEEP_LAST , #keep only last N samples
63+ depth = 0 , #No need to maintain history
64+ reliability = QoSReliabilityPolicy .BEST_EFFORT , #will accept a reliable or best effort publisher
65+ durability = QoSDurabilityPolicy .VOLATILE , #Do not need persistent samples (but will still accept if pub chooses)
66+ liveliness = QoSLivelinessPolicy .AUTOMATIC , #Automatically detect liveliness change on lease duration exceeded
67+ liveliness_lease_duration = BEAT_LOST_DURATION , #Max duration between individual messages before beat considered lost
68+ deadline = BEAT_MISSED_DEADLINE #max duration before single beat considered missed
69+ )
6370
64- #use best effort to be flexible (allow pub to either be best effort or reliable)
65- #https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html#qos-policies
66- qos_profile .reliability = rclpy .qos .QoSReliabilityPolicy .BEST_EFFORT
67-
68- #Samples should not be persistent
69- #N.B. if publisher chooses to make them consistent then this is still allowed
70- qos_profile .durability = rclpy .qos .QoSDurabilityPolicy .VOLATILE
71-
72- #automatic liveliness detection so automatically detect when lease duration exceeded
73- qos_profile .liveliness = rclpy .qos .QoSLivelinessPolicy .AUTOMATIC
7471 #N.B. for QoS compatability a publisher must promise a QoS with
75- #lease duration shorter than the lease expected here
76- qos_profile .liveliness_lease_duration = BEAT_LOST_DURATION
77-
78- #set deadline for each beat. if deadline not met log warning
72+ #lease duration shorter than the lease expected in QoSProfile
7973 #N.B. that for QoS compatability a publisher must promise a QoS
80- #with deadline shorter than the deadline expected in this subscriber
81- qos_profile .deadline = BEAT_MISSED_DEADLINE
74+ #with deadline shorter than the deadline expected in this QoSProfile
8275
8376 #create event callbacks to be triggerred on deadline missed
8477 #and lease duration execeeded (liveliness lost)
0 commit comments