3 from message_filters
import SimpleFilter
7 Synchronize multiple topics using a simple 8 and non-optimal strategy. 10 This caches messages for all subscribed filters 11 and passes them on (bundled) whenever there are 12 messages available on all topics. 15 def __init__(self, filters, queue_size=10, max_difference=rospy.Duration(.5)):
17 :param queue_size: The size of the message buffer for each filter 18 :param max_difference: The maximal time difference between messages in the same bundle 21 SimpleFilter.__init__(self)
26 self.
lock = threading.Lock()
33 def add(self, msg, queue):
41 if len(queue) == 1
and not([]
in self.
queues):
44 t= queue[0].header.stamp
46 def mindiff(queue, time):
47 """ return the message closest to time """ 50 if abs(msg.header.stamp - time) < abs(selmsg.header.stamp - time):
52 elif abs(msg.header.stamp - time) > abs(selmsg.header.stamp - time):
58 msgs= [mindiff(q, t)
for q
in self.
queues]
59 times= [m.header.stamp
for m
in msgs]
61 bundle_size= max(times)-min(times)
65 rospy.loginfo(
"discard bundle because overall time difference %s > %s" % (str(bundle_size), str(self.
max_diff)))
68 for (q,t)
in zip(self.
queues, times):
69 while len(q) > 0
and q[0].header.stamp <= t:
void signalMessage(const MConstPtr &msg)
def add(self, msg, queue)
def connectInput(self, filters)
def __init__(self, filters, queue_size=10, max_difference=rospy.Duration(.5))