rect_array_to_polygon_array.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import dynamic_reconfigure.server
4 from geometry_msgs.msg import PolygonStamped
5 from geometry_msgs.msg import Point32
6 from jsk_recognition_msgs.msg import PolygonArray
7 from jsk_recognition_msgs.msg import RectArray
8 from jsk_topic_tools import ConnectionBasedTransport
9 from jsk_recognition_utils.cfg import PolygonArrayToPolygonConfig
10 import rospy
11 
12 
13 class RectArrayToPolygonArray(ConnectionBasedTransport):
14 
15  def __init__(self):
16  super(RectArrayToPolygonArray, self).__init__()
17  self.pub = self.advertise('~output', PolygonArray, queue_size=1)
18  dynamic_reconfigure.server.Server(PolygonArrayToPolygonConfig,
19  self._config_callback)
20 
21  def subscribe(self):
22  self.sub = rospy.Subscriber('~input', RectArray, self._convert)
23 
24  def unsubscribe(self):
25  self.sub.unregister()
26 
27  def _config_callback(self, config, level):
28  self.index = config.index
29  return config
30 
31  def _convert(self, msg):
32  polys_msg = PolygonArray()
33  polys_msg.header = msg.header
34  for rect in msg.rects:
35  poly_msg = PolygonStamped()
36  poly_msg.header = msg.header
37  pt0 = Point32(x=rect.x, y=rect.y)
38  pt1 = Point32(x=rect.x, y=rect.y + rect.height)
39  pt2 = Point32(x=rect.x + rect.width, y=rect.y + rect.height)
40  pt3 = Point32(x=rect.x + rect.width, y=rect.y)
41  poly_msg.polygon.points.append(pt0)
42  poly_msg.polygon.points.append(pt1)
43  poly_msg.polygon.points.append(pt2)
44  poly_msg.polygon.points.append(pt3)
45  polys_msg.polygons.append(poly_msg)
46  self.pub.publish(polys_msg)
47 
48 
49 if __name__ == '__main__':
50  rospy.init_node('rect_array_to_polygon_array')
52  rospy.spin()


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