Repository: ros/ros_comm

Issue:
[message_filters] Python interface of approximate synchronizer hangs up with large queue size
## Issue

Topic's fps goes down. When I use the message_filter's approximate synchronizer with large queue size (100) and processing in the callback which takes time (1 second).

```
% roslaunch jsk_20160505_test_message_filters string.launch

% rostopic hz /join_strings/output
subscribed to [/join_strings/output]
average rate: 0.997
        min: 1.003s max: 1.003s std dev: 0.00000s window: 2
average rate: 0.996
        min: 1.003s max: 1.004s std dev: 0.00019s window: 3
average rate: 0.975
        min: 1.003s max: 1.070s std dev: 0.03121s window: 4
average rate: 0.962
        min: 1.003s max: 1.079s std dev: 0.03554s window: 5
average rate: 0.949
        min: 1.003s max: 1.112s std dev: 0.04305s window: 6
average rate: 0.925
        min: 1.003s max: 1.219s std dev: 0.07304s window: 7
average rate: 0.915
        min: 1.003s max: 1.219s std dev: 0.07346s window: 8
no new messages
average rate: 0.887
        min: 1.003s max: 1.373s std dev: 0.11528s window: 9
average rate: 0.871
        min: 1.003s max: 1.373s std dev: 0.12357s window: 10
no new messages
average rate: 0.786
        min: 1.003s max: 2.381s std dev: 0.38777s window: 11
no new messages
no new messages
no new messages
no new messages
average rate: 0.621
        min: 1.003s max: 5.008s std dev: 1.13588s window: 12
no new messages
no new messages
average rate: 0.579
        min: 1.003s max: 5.008s std dev: 1.15229s window: 13
no new messages
no new messages
no new messages
no new messages
average rate: 0.519
        min: 1.003s max: 5.008s std dev: 1.30438s window: 14
no new messages
no new messages
no new messages
average rate: 0.470
        min: 1.003s max: 5.008s std dev: 1.45689s window: 15
no new messages
```
## Testing Code
- Package: https://github.com/wkentaro/jsk_20160505_test_message_filters
- string.launch: https://github.com/wkentaro/jsk_20160505_test_message_filters/blob/master/launch/string.launch
- publish_string: https://github.com/wkentaro/jsk_20160505_test_message_filters/blob/master/scripts/publish_string
- join_strings: https://github.com/wkentaro/jsk_20160505_test_message_filters/blob/master/scripts/join_strings  (sync 3 topics with 1sec sleep in callback)
## Solution

After some workaround with print debugging, I found this section takes time when the program hangs up.
https://github.com/ros/ros_comm/blob/kinetic-devel/utilities/message_filters/src/message_filters/__init__.py#L224-L231 
So solutions are:
- Refactor code for searching the timestamp to do it with pure python #802 
- Use numpy for searching the timestamp #801 



Discussion:

Resolving patch:
diff --git a/utilities/message_filters/src/message_filters/__init__.py b/utilities/message_filters/src/message_filters/__init__.py
--- a/utilities/message_filters/src/message_filters/__init__.py
+++ b/utilities/message_filters/src/message_filters/__init__.py
@@ -34,6 +34,7 @@
 import threading
 import rospy
 
+
 class SimpleFilter(object):
 
     def __init__(self):
@@ -202,9 +203,11 @@ def __init__(self, fs, queue_size):
 
     def connectInput(self, fs):
         self.queues = [{} for f in fs]
-        self.input_connections = [f.registerCallback(self.add, q) for (f, q) in zip(fs, self.queues)]
+        self.input_connections = [
+            f.registerCallback(self.add, q, i_q)
+            for i_q, (f, q) in enumerate(zip(fs, self.queues))]
 
-    def add(self, msg, my_queue):
+    def add(self, msg, my_queue, my_queue_index=None):
         self.lock.acquire()
         my_queue[msg.header.stamp] = msg
         while len(my_queue) > self.queue_size:
@@ -237,7 +240,7 @@ def __init__(self, fs, queue_size, slop, allow_headerless=False):
         self.slop = rospy.Duration.from_sec(slop)
         self.allow_headerless = allow_headerless
 
-    def add(self, msg, my_queue):
+    def add(self, msg, my_queue, my_queue_index=None):
         if not hasattr(msg, 'header') or not hasattr(msg.header, 'stamp'):
             if not self.allow_headerless:
                 rospy.logwarn("Cannot use message filters with non-stamped messages. "
@@ -252,7 +255,31 @@ def add(self, msg, my_queue):
         my_queue[stamp] = msg
         while len(my_queue) > self.queue_size:
             del my_queue[min(my_queue)]
-        for vv in itertools.product(*[list(q.keys()) for q in self.queues]):
+        # self.queues = [topic_0 {stamp: msg}, topic_1 {stamp: msg}, ...]
+        if my_queue_index is None:
+            search_queues = self.queues
+        else:
+            search_queues = self.queues[:my_queue_index] + \
+                self.queues[my_queue_index+1:]
+        # sort and leave only reasonable stamps for synchronization
+        stamps = []
+        for queue in search_queues:
+            topic_stamps = []
+            for s in queue:
+                stamp_delta = abs(s - stamp)
+                if stamp_delta > self.slop:
+                    continue  # far over the slop
+                topic_stamps.append((s, stamp_delta))
+            if not topic_stamps:
+                self.lock.release()
+                return
+            topic_stamps = sorted(topic_stamps, key=lambda x: x[1])
+            stamps.append(topic_stamps)
+        for vv in itertools.product(*[zip(*s)[0] for s in stamps]):
+            vv = list(vv)
+            # insert the new message
+            if my_queue_index is not None:
+                vv.insert(my_queue_index, stamp)
             qt = list(zip(self.queues, vv))
             if ( ((max(vv) - min(vv)) < self.slop) and
                 (len([1 for q,t in qt if t not in q]) == 0) ):
@@ -260,4 +287,5 @@ def add(self, msg, my_queue):
                 self.signalMessage(*msgs)
                 for q,t in qt:
                     del q[t]
+                break  # fast finish after the synchronization
         self.lock.release()


Test patch: