synchronize_republish.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 import message_filters
5 
6 from rostopic import get_topic_class
7 
8 
9 def callback(*msgs):
10  stamp = None
11  for msg, pub in zip(msgs, pubs):
12  if stamp is None:
13  stamp = msg.header.stamp
14  else:
15  msg.header.stamp = stamp
16  pub.publish(msg)
17 
18 
19 if __name__ == '__main__':
20  rospy.init_node('synchrnoze_republish')
21  topics = rospy.get_param('~topics')
22  use_async = rospy.get_param('~approximate_sync', False)
23  pubs = []
24  subs = []
25  for i, topic in enumerate(topics):
26  topic = rospy.resolve_name(topic)
27  msg_class = get_topic_class(topic, blocking=True)[0]
28  pub = rospy.Publisher(
29  '~pub_{0:0>2}'.format(i), msg_class, queue_size=1)
30  pubs.append(pub)
31  sub = message_filters.Subscriber(topic, msg_class)
32  subs.append(sub)
33  if use_async:
34  sync = message_filters.ApproximateTimeSynchronizer(
35  subs, queue_size=100, slop=0.1)
36  else:
37  sync = message_filters.TimeSynchronizer(subs, queue_size=100)
38  sync.registerCallback(callback)
39  rospy.spin()


jsk_topic_tools
Author(s): Kei Okada , Yusuke Furuta
autogenerated on Tue Feb 6 2018 03:45:19