rect_array_to_cluster_point_indices.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import numpy as np
4 
5 import message_filters
6 import rospy
7 
8 from jsk_recognition_msgs.msg import ClusterPointIndices
9 from jsk_recognition_msgs.msg import RectArray
10 from jsk_topic_tools import ConnectionBasedTransport
11 from pcl_msgs.msg import PointIndices
12 from sensor_msgs.msg import CameraInfo
13 
14 
15 class RectArrayToClusterPointIndices(ConnectionBasedTransport):
16 
17  def __init__(self):
18  super(RectArrayToClusterPointIndices, self).__init__()
19  self.pub = self.advertise('~output', ClusterPointIndices, queue_size=1)
20 
21  def subscribe(self):
22  if rospy.get_param('~use_info', False):
23  sub_rects = message_filters.Subscriber('~input', RectArray)
24  sub_info = message_filters.Subscriber('~input/info', CameraInfo)
25  self.subs = [sub_rects, sub_info]
26  queue_size = rospy.get_param('~queue_size', 10)
27  if rospy.get_param('~approximate_sync', False):
28  slop = rospy.get_param('~slop', 0.1)
29  sync = message_filters.ApproximateTimeSynchronizer(
30  self.subs, queue_size=queue_size, slop=slop)
31  else:
33  self.subs, queue_size=queue_size)
34  sync.registerCallback(self._convert_sync)
35  else:
36  self.img_height = rospy.get_param('~img_height')
37  self.img_width = rospy.get_param('~img_width')
38  sub_rects = rospy.Subscriber('~input', RectArray, self._convert)
39  self.subs = [sub_rects]
40 
41  def unsubscribe(self):
42  for sub in self.subs:
43  sub.unregister()
44 
45  def _convert_sync(self, rects_msg, info_msg):
46  H = info_msg.height
47  W = info_msg.width
48  self._convert(rects_msg, H, W)
49 
50  def _convert(self, rects_msg, img_height=None, img_width=None):
51  H = self.img_height if img_height is None else img_height
52  W = self.img_width if img_width is None else img_width
53  cpi_msg = ClusterPointIndices(header=rects_msg.header)
54  for rect in rects_msg.rects:
55  indices_msg = PointIndices(header=rects_msg.header)
56  ymin = max(0, int(np.floor(rect.y)))
57  xmin = max(0, int(np.floor(rect.x)))
58  ymax = min(H, int(np.ceil(rect.y + rect.height)))
59  xmax = min(W, int(np.ceil(rect.x + rect.width)))
60  indices = [range(W*y+xmin, W*y+xmax) for y in range(ymin, ymax)]
61  indices_msg.indices = np.array(indices, dtype=np.int32).flatten()
62  cpi_msg.cluster_indices.append(indices_msg)
63  self.pub.publish(cpi_msg)
64 
65 
66 if __name__ == '__main__':
67  rospy.init_node('rect_array_to_cluster_point_indices')
69  rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Sat Feb 6 2021 04:07:56