Repository: ros/ros_comm

Issue:
message_filters: Incorporate python approximate time synchronizer
https://github.com/ros-perception/image_pipeline/issues/76



Discussion:
Please consider providing a pull request for this feature and also contribute to the documentation for it.

Well, it is already here :)
https://github.com/ros/ros_comm/blob/indigo-devel/utilities/message_filters/src/message_filters/__init__.py#L101
So you can close this one.

sorry, I missed it, this is approximate: should I create a new class or just add a slop parameter like done in the camera_calibration file ? (that would default to infinity I guess)

A parameter seems reasonable. though i'd make the slop default to zero ;-)

For ease of use it might be worth implementing it in the regular syncronizer using a semi protected method, and then have a separate wrapper class which invokes that code path and provides the clear interface.


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
@@ -30,6 +30,7 @@
 ======================
 """
 
+import itertools
 import threading
 import rospy
 
@@ -143,3 +144,33 @@ def add(self, msg, my_queue):
             for q in self.queues:
                 del q[t]
         self.lock.release()
+
+class ApproximateTimeSynchronizer(TimeSynchronizer):
+
+    """
+    Approximately synchronizes messages by their timestamps.
+
+    :class:`ApproximateTimeSynchronizer` synchronizes incoming message filters by the
+    timestamps contained in their messages' headers. The API is the same as TimeSynchronizer
+    except for an extra `slop` parameter in the constructor that defines the delay (in seconds)
+    with which messages can be synchronized
+    """
+
+    def __init__(self, fs, queue_size, slop):
+        TimeSynchronizer.__init__(self, fs, queue_size)
+        self.slop = rospy.Duration.from_sec(slop)
+
+    def add(self, msg, my_queue):
+        self.lock.acquire()
+        my_queue[msg.header.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]):
+            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) ):
+                msgs = [q[t] for q,t in qt]
+                self.signalMessage(*msgs)
+                for q,t in qt:
+                    del q[t]
+        self.lock.release()


Test patch:
diff --git a/utilities/message_filters/test/test_approxsync.py b/utilities/message_filters/test/test_approxsync.py
new file mode 100644
--- /dev/null
+++ b/utilities/message_filters/test/test_approxsync.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rostest
+import rospy
+import unittest
+import random
+
+import message_filters
+from message_filters import ApproximateTimeSynchronizer
+
+class MockHeader:
+    pass
+
+class MockMessage:
+    def __init__(self, stamp, data):
+        self.header = MockHeader()
+        self.header.stamp = stamp
+        self.data = data
+
+class MockFilter(message_filters.SimpleFilter):
+    pass
+
+class TestApproxSync(unittest.TestCase):
+
+    def cb_collector_2msg(self, msg1, msg2):
+        self.collector.append((msg1, msg2))
+
+    def test_approx(self):
+        m0 = MockFilter()
+        m1 = MockFilter()
+        ts = ApproximateTimeSynchronizer([m0, m1], 1, 0.1)
+        ts.registerCallback(self.cb_collector_2msg)
+
+        if 0:
+            # Simple case, pairs of messages, make sure that they get combined
+            for t in range(10):
+                self.collector = []
+                msg0 = MockMessage(t, 33)
+                msg1 = MockMessage(t, 34)
+                m0.signalMessage(msg0)
+                self.assertEqual(self.collector, [])
+                m1.signalMessage(msg1)
+                self.assertEqual(self.collector, [(msg0, msg1)])
+
+        # Scramble sequences of length N.  Make sure that TimeSequencer recombines them.
+        random.seed(0)
+        for N in range(1, 10):
+            m0 = MockFilter()
+            m1 = MockFilter()
+            seq0 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
+            seq1 = [MockMessage(rospy.Time(t), random.random()) for t in range(N)]
+            # random.shuffle(seq0)
+            ts = ApproximateTimeSynchronizer([m0, m1], N, 0.1)
+            ts.registerCallback(self.cb_collector_2msg)
+            self.collector = []
+            for msg in random.sample(seq0, N):
+                m0.signalMessage(msg)
+            self.assertEqual(self.collector, [])
+            for msg in random.sample(seq1, N):
+                m1.signalMessage(msg)
+            self.assertEqual(set(self.collector), set(zip(seq0, seq1)))
+
+if __name__ == '__main__':
+    if 1:
+        rostest.unitrun('camera_calibration', 'testapproxsync', TestApproxSync)
+    else:
+        suite = unittest.TestSuite()
+        suite.addTest(TestApproxSync('test_approx'))
+        unittest.TextTestRunner(verbosity=2).run(suite)