jsk_topic_tools: synchronize_republish.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import message_filters
00005 
00006 from rostopic import get_topic_class
00007 
00008 
00009 def callback(*msgs):
00010     stamp = None
00011     for msg, pub in zip(msgs, pubs):
00012         if stamp is None:
00013             stamp = msg.header.stamp
00014         else:
00015             msg.header.stamp = stamp
00016         pub.publish(msg)
00017 
00018 
00019 if __name__ == '__main__':
00020     rospy.init_node('synchrnoze_republish')
00021     topics = rospy.get_param('~topics')
00022     use_async = rospy.get_param('~approximate_sync', False)
00023     pubs = []
00024     subs = []
00025     for i, topic in enumerate(topics):
00026         topic = rospy.resolve_name(topic)
00027         msg_class = get_topic_class(topic, blocking=True)[0]
00028         pub = rospy.Publisher(
00029             '~pub_{0:0>2}'.format(i), msg_class, queue_size=1)
00030         pubs.append(pub)
00031         sub = message_filters.Subscriber(topic, msg_class)
00032         subs.append(sub)
00033     if use_async:
00034         sync = message_filters.ApproximateTimeSynchronizer(
00035             subs, queue_size=100, slop=0.1)
00036     else:
00037         sync = message_filters.TimeSynchronizer(subs, queue_size=100)
00038     sync.registerCallback(callback)
00039     rospy.spin()