polygon_array_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 import rospy
5 from geometry_msgs.msg import PolygonStamped, Point32
6 from jsk_recognition_msgs.msg import PolygonArray
7 
8 
9 class PolygonArrayPublisher(object):
10  def __init__(self):
11  publish_rate = rospy.get_param("~publish_rate", 1.0)
12 
13  frame_id = rospy.get_param("~frame_id")
14  param = rospy.get_param("~polygons")
15  self.msg = self.parse_params(param, frame_id)
16 
17  self.pub_polygons = rospy.Publisher("~output", PolygonArray, queue_size=1)
18 
19  self.timer = rospy.Timer(rospy.Duration(1. / publish_rate), self.publish)
20 
21  def parse_params(self, param, frame_id):
22  # validate params
23  assert isinstance(param, list), "polygons must be list"
24 
25  has_label = any("label" in p for p in param)
26  has_likelihood = any("likelihood" in p for p in param)
27 
28  for polygon in param:
29  assert "points" in polygon and isinstance(polygon["points"], list),\
30  "each polygon must have at least 1 'points'"
31  for point in polygon["points"]:
32  assert len(point) == 3,\
33  "element of 'points' must be list of 3 numbers"
34  if has_label:
35  assert "label" in polygon, "missing 'label'"
36  if has_likelihood:
37  assert "likelihood" in polygon, "missing 'likelihood'"
38 
39  # parse params
40  msg = PolygonArray()
41  msg.header.frame_id = frame_id
42  for polygon in param:
43  ps = PolygonStamped()
44  ps.header.frame_id = frame_id
45  ps.polygon.points = [Point32(*v) for v in polygon["points"]]
46  msg.polygons.append(ps)
47  if has_label:
48  msg.labels.append(polygon["label"])
49  if has_likelihood:
50  msg.likelihood.append(polygon["likelihood"])
51 
52  return msg
53 
54  def publish(self, event):
55  # update timestamp
56  self.msg.header.stamp = event.current_real
57  for p in self.msg.polygons:
58  p.header.stamp = event.current_real
59 
60  self.pub_polygons.publish(self.msg)
61 
62 if __name__ == '__main__':
63  rospy.init_node("polygon_array_publisher")
65  rospy.spin()


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