polygon_array_to_polygon.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from __future__ import absolute_import
4 from __future__ import division
5 from __future__ import print_function
6 
7 import dynamic_reconfigure.server
8 from geometry_msgs.msg import PolygonStamped
9 from jsk_recognition_msgs.msg import PolygonArray
10 from jsk_topic_tools import ConnectionBasedTransport
11 from jsk_topic_tools.log_utils import logerr_throttle
12 from jsk_recognition_utils.cfg import PolygonArrayToPolygonConfig
13 import rospy
14 
15 
16 class PolygonArrayToPolygon(ConnectionBasedTransport):
17 
18  def __init__(self):
19  super(self.__class__, self).__init__()
20  self.pub = self.advertise('~output', PolygonStamped, queue_size=1)
21  dynamic_reconfigure.server.Server(PolygonArrayToPolygonConfig,
22  self._config_callback)
23 
24  def subscribe(self):
25  self.sub = rospy.Subscriber('~input', PolygonArray, self._convert)
26 
27  def unsubscribe(self):
28  self.sub.unregister()
29 
30  def _config_callback(self, config, level):
31  self.index = config.index
32  return config
33 
34  def _convert(self, msg):
35  polygon_msg = PolygonStamped()
36  polygon_msg.header = msg.header
37  if self.index < 0:
38  return
39  elif self.index < len(msg.polygons):
40  polygon_msg = msg.polygons[self.index]
41  self.pub.publish(polygon_msg)
42  else:
43  logerr_throttle(10,
44  'Invalid index {} is specified '
45  'for polygon array whose size is {}'
46  .format(self.index, len(msg.polygons)))
47 
48 
49 if __name__ == '__main__':
50  rospy.init_node('polygon_array_to_polygon')
52  rospy.spin()


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