ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Why do I need a sleep before publishing first message to a topic?

asked 2021-12-19 18:06:52 -0500

ajayvohra2005 gravatar image

I am trying to get an explanation for some code that works.

To simplify the general issue, I created a bag file with exactly 2 messages with exact same header time stamp down to nano seconds. First message is of type sensor_msgs/Image. Second message is of type sensor_msgs/PointCloud2. If I read the two messages from the ROS Bag and publish them using two publishers, one for each topic, as shown below, I can not see the messages in rviz or rostopic echo.

#The code below does not show messages in rviz or rostopic echo 
# ros_publishers in the code below is a dict of ros topic name -> rospy.Publisher
bag = rosbag.Bag(bag_path)
for ros_topic, ros_msg, _ in bag.read_messages():
  ros_publishers[ros_topic].publish(ros_msg)
bag.close()

However, if I add a minimum sleep for 1 second, just before I publish a message on a topic for the first time, it works.

# This code successfully shows messages in rviz and rostopic echo
# ros_publishers is a dict of ros topic name -> rospy.Publisher
bag = rosbag.Bag(bag_path)
ros_topics = set()
for ros_topic, ros_msg, _ in bag.read_messages():
   if not ros_topic in ros_topics:
     time.sleep(1)
     ros_topics.add(ros_topic)
   ros_publishers[ros_topic].publish(ros_msg)
bag.close()

This issue is a proxy for the real issue, which is that if number of messages in the Bag file is below a threshold (don't know the threshold), the code needs the sleep for first time message on a topic before the message shows up in rviz or rostopic echo. If the number of messages in the Bag file is a above this unknown threshold, then it works without any sleep.

Interestingly, using rosbag play shows messages in rviz even if there are just two messages in the Bag, but that may or may not be beside the point for the problem I am trying to solve. I can change the time.sleep(1) to print(str(ros_msg)) and that does the trick also. Note, if I put the time.sleep(1) anywhere else in the code, it does not work, and less than 1 second also does not work.

I have a solution (maybe a hack) that works for what I am trying to do, but I am requesting an explanation for why it works, or maybe I need to do something else.

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
4

answered 2021-12-20 04:45:09 -0500

updated 2021-12-20 04:47:14 -0500

TL;DR: I strongly suspect that what's happening is that you create a publisher and immediately publish a message on it before the subscriber has had time to connect, so the first message (or first few messages) are missed.

Longer version:

Let's call the node with your code that you pasted above Talker and the node that subscribes Listener.

Here's the sequence of events:

  1. The ROS master (roscore) is started.
  2. Listener is started.
  3. At the moment that the Subscriber is created for topic T in the Listener code, it notifies the master that it would like to be notified of any future publishers on that topic.
  4. Talker is started.
  5. At the moment that the Publisher for topic T is created, the master is notified.
  6. The master notifies Listener that there is a new publisher and tells it the IP and port of the publisher in the Talker node.
  7. Talker publishes its first message (or couple of messages). They are missed by the Listener.
  8. Listener contacts Talker and requests that future messages are send to it.
  9. Talker keeps publishing messages. They are received by Listener.

If you add a sleep between step 6 and 7 (i.e., after creating the publisher, but before publishing messages), that gives the subscribers time to subscribe, and no messages are missed.


A second point is that you are publishing messages as fast as you can read them from disk, without delay. This can result in tens of thousands of messages being published per second. In that case, either the publisher or subscriber queue will overflow, and messages are dropped.

In summary, you should do the following:

  • create a small sleep (1 second is more than enough) after creating the publishers, but before starting to publish if it's important not to miss any messages.
  • Ensure that the queue_size parameters in all publishers and subscribers are large enough to buffer a few seconds worth of messages.
  • Rate limit your publishing to ensure that you don't publish more than queue_size / N messages per second, where N is something like 1..5.
edit flag offensive delete link more

Comments

Thank you for the precise explanation. This solved my problem elegantly.

ajayvohra2005 gravatar image ajayvohra2005  ( 2021-12-21 17:49:10 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2021-12-19 17:28:06 -0500

Seen: 402 times

Last updated: Dec 20 '21