Go to the documentation of this file.00001
00002
00003
00004 from jsk_recognition_msgs.msg import ClusterPointIndices
00005 from jsk_topic_tools import ConnectionBasedTransport
00006 import message_filters
00007 import rospy
00008
00009
00010 class AddClusterIndices(ConnectionBasedTransport):
00011 def __init__(self):
00012 super(AddClusterIndices, self).__init__()
00013 self._pub = self.advertise('~output', ClusterPointIndices,
00014 queue_size=1)
00015
00016 def subscribe(self):
00017 topics = rospy.get_param('~topics')
00018 self.subs = []
00019 for tp in topics:
00020 sub = message_filters.Subscriber(tp, ClusterPointIndices)
00021 self.subs.append(sub)
00022 use_async = rospy.get_param('~approximate_sync', False)
00023 queue_size = rospy.get_param('~queue_size', 100)
00024 if use_async:
00025 slop = rospy.get_param('~slop', 0.1)
00026 sync = message_filters.ApproximateTimeSynchronizer(
00027 self.subs, queue_size, slop)
00028 else:
00029 sync = message_filters.TimeSynchronizer(self.subs, queue_size)
00030 sync.registerCallback(self._decompose)
00031
00032 def unsubscribe(self):
00033 for sub in self.subs:
00034 sub.unregister()
00035
00036 def _decompose(self, *cpi_msgs):
00037 out_msg = ClusterPointIndices()
00038 out_msg.header = cpi_msgs[0].header
00039 for cpi_msg in cpi_msgs:
00040 out_msg.cluster_indices.extend(cpi_msg.cluster_indices)
00041 self._pub.publish(out_msg)
00042
00043
00044 if __name__ == '__main__':
00045 rospy.init_node('add_cluster_indices')
00046 app = AddClusterIndices()
00047 rospy.spin()