(self, msg, my_queue)
| 39 | self.input_connections = [f.registerCallback(self.add, q) for (f, q) in zip(fs, self.queues)] |
| 40 | |
| 41 | def add(self, msg, my_queue): |
| 42 | self.lock.acquire() |
| 43 | time = rospy.Time(msg.header.stamp.secs, msg.header.stamp.nsecs) |
| 44 | duration = (self.last_time - time).to_sec() |
| 45 | self.last_time = time |
| 46 | if duration > 15.0: |
| 47 | print 'timestamp skiped to the past!!\nclear the synchronization queue!!' |
| 48 | print 'time difference', -duration, ' [sec]' |
| 49 | while len(my_queue): |
| 50 | my_queue.clear() |
| 51 | for queue in self.queues: |
| 52 | queue.clear() |
| 53 | self.lock.release() |
| 54 | return |
| 55 | |
| 56 | msg.header.stamp.nsecs = (msg.header.stamp.nsecs / self.nsecs_round) * self.nsecs_round |
| 57 | my_queue[msg.header.stamp] = msg |
| 58 | while len(my_queue) > self.queue_size: |
| 59 | del my_queue[min(my_queue)] |
| 60 | # common is the set of timestamps that occur in all queues |
| 61 | common = reduce(set.intersection, [set(q) for q in self.queues]) |
| 62 | for t in sorted(common): |
| 63 | # msgs is list of msgs (one from each queue) with stamp t |
| 64 | msgs = [q[t] for q in self.queues] |
| 65 | self.signalMessage(*msgs) |
| 66 | for q in self.queues: |
| 67 | del q[t] |
| 68 | self.lock.release() |
no test coverage detected