add_cluster_indices.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 from jsk_recognition_msgs.msg import ClusterPointIndices
5 from jsk_topic_tools import ConnectionBasedTransport
6 import message_filters
7 import rospy
8 
9 
10 class AddClusterIndices(ConnectionBasedTransport):
11  def __init__(self):
12  super(AddClusterIndices, self).__init__()
13  self._pub = self.advertise('~output', ClusterPointIndices,
14  queue_size=1)
15 
16  def subscribe(self):
17  topics = rospy.get_param('~topics')
18  self.subs = []
19  for tp in topics:
20  sub = message_filters.Subscriber(tp, ClusterPointIndices)
21  self.subs.append(sub)
22  use_async = rospy.get_param('~approximate_sync', False)
23  queue_size = rospy.get_param('~queue_size', 100)
24  if use_async:
25  slop = rospy.get_param('~slop', 0.1)
26  sync = message_filters.ApproximateTimeSynchronizer(
27  self.subs, queue_size, slop)
28  else:
29  sync = message_filters.TimeSynchronizer(self.subs, queue_size)
30  sync.registerCallback(self._decompose)
31 
32  def unsubscribe(self):
33  for sub in self.subs:
34  sub.unregister()
35 
36  def _decompose(self, *cpi_msgs):
37  out_msg = ClusterPointIndices()
38  out_msg.header = cpi_msgs[0].header
39  for cpi_msg in cpi_msgs:
40  out_msg.cluster_indices.extend(cpi_msg.cluster_indices)
41  self._pub.publish(out_msg)
42 
43 
44 if __name__ == '__main__':
45  rospy.init_node('add_cluster_indices')
47  rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Fri Dec 6 2019 04:02:51