Repository: ros/ros_comm

Issue:
Using queue_size in a rospy publisher causes slowdown when there are multiple subscribers
When a rospy publisher uses a queue_size, if there are multiple subscribers (more than 2), I see lots of dropped messages. This happens even when the messages are small, and the CPU load is nowhere near maxed. It happens even if the queue_size is very large.  To reproduce, use this `pub` script:

``` python
#!/usr/bin/env python
import rospy
import sys
from std_msgs.msg import String

pub_rate = int(sys.argv[1])
queue_size = None
if len(sys.argv) > 2:
    queue_size = int(sys.argv[2])

rospy.init_node('pub', anonymous=True)
pub = rospy.Publisher('foo', String, queue_size = queue_size)

r = rospy.Rate(pub_rate)
while not rospy.is_shutdown():
    pub.publish(String('helloooooooo'))
    r.sleep()
```

In one terminal, do `./pub 150 1000` to publish messages at 150 Hz on topic "foo" with a queue_size of 1000. Then in open 3 or 4 other terminals and run `rostopic hz /foo -w200`. For each rostopic hz you add, the rate drops. On my computer, with 3 of them, I get 100-130 hz instead of 150hz.  Checking top shows only 12% CPU usage for the publisher node.

If you run the same experiment, but start the publisher with `./pub 150`, it will create the publisher without queue_size, and you should see all subscribers getting 150 hz.

Since the publisher doesn't seem to be CPU limited or network limited, I'm not sure what is going on. Maybe lock contention somewhere?



Discussion:
(i'm running this all on one machine, with everything talking to localhost)


Resolving patch:
diff --git a/clients/rospy/src/rospy/impl/tcpros_pubsub.py b/clients/rospy/src/rospy/impl/tcpros_pubsub.py
--- a/clients/rospy/src/rospy/impl/tcpros_pubsub.py
+++ b/clients/rospy/src/rospy/impl/tcpros_pubsub.py
@@ -376,9 +376,7 @@ def __init__(self, connection, queue_size):
 
         self._lock = threading.Lock()
         self._cond_data_available = threading.Condition(self._lock)
-        self._cond_queue_swapped = threading.Condition(self._lock)
         self._queue = []
-        self._waiting = False
         self._error = None
 
         self._thread = threading.Thread(target=self._run)
@@ -401,11 +399,8 @@ def write_data(self, data):
                 del self._queue[0]
             self._queue.append(data)
             self._cond_data_available.notify()
-            # ensure that thread has actually swapped the queues and is processig them
-            # if it was waiting for being notified
-            # to enforce behavior to be as close as possible to blocking
-            if self._waiting:
-                self._cond_queue_swapped.wait()
+        # effectively yields the rest of the thread quantum
+        time.sleep(0)
         return True
 
     def _run(self):
@@ -414,11 +409,7 @@ def _run(self):
             with self._lock:
                 # wait for available data
                 while not self._queue and not self._connection.done:
-                    self._waiting = True
                     self._cond_data_available.wait(1.0)
-                    self._waiting = False
-                    if self._queue:
-                        self._cond_queue_swapped.notify()
                 # take all data from queue for processing outside of the lock
                 if self._queue:
                     queue = self._queue


Test patch: