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


jsk_recognition_utils
Author(s):
autogenerated on Wed Mar 3 2021 04:00:53