Cleanly exit from python publisher ensuring all messages sent
I have been working on a Python node that essentially publishes one-off messages and then exits. Due, I believe, to the threading in rospy, messages are not always sent if the script exits soon after publishing a message. I have also found no ways of truly guaranteeing that messages that sent messages are received.
Here is some example code based roughly on the nodes demonstrated in the ROS tutorials:
Publisher
#!/usr/bin/python
from std_msgs.msg import Header
import rospy
import math
topic = 'foo'
publisher = rospy.Publisher(topic, Header, queue_size = 10)
rospy.init_node('publisher',anonymous = True)
msg = Header()
msg.frame_id = "Hello World!"
publisher.publish(msg)
Subscriber:
#!/usr/bin/python
import rospy
from std_msgs.msg import Header
def callback(data):
rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.frame_id)
def listener():
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("foo", Header, callback)
rospy.spin()
if __name__ == '__main__':
listener()
There are a number of modifications I have found that will get the messages to send:
- Use a rospy.Rate object with sufficiently low rate and sleep after publishing
- Sending messages in a loop with no other calls to ROS functionality will result in most messages being received.
- edit I tried a loop
while not rorpy.is_shutdown()
containingrospy.signal_shutdown('stuff')
after the message is sent. This method results in the message being received some of the time.
There are also some exceptions where the message may otherwise be expected to be received:
- Sleeping using a rospy.Duration object will not allow sends to complete
- rospy.signal_shutdown does not guarantee that pending messages are sent
- Pending sends are not completed when using a Rate object with too high a rate
On top of all this, I have found that rostopic echo
will often (but not always) get messages when a subscribing node does not.
I would like to know if there is any (hopefully documented) way that I can exit a python script with the guarantee that every subscriber to a topic that node publishes will have received every message that has been published. So far I have not been able to find anything to this effect.