add_cluster_indices.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
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()


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:36