polygon_array_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 import rospy
00005 from geometry_msgs.msg import PolygonStamped, Point32
00006 from jsk_recognition_msgs.msg import PolygonArray
00007 
00008 
00009 class PolygonArrayPublisher(object):
00010     def __init__(self):
00011         publish_rate = rospy.get_param("~publish_rate", 1.0)
00012 
00013         frame_id = rospy.get_param("~frame_id")
00014         param = rospy.get_param("~polygons")
00015         self.msg = self.parse_params(param, frame_id)
00016 
00017         self.pub_polygons = rospy.Publisher("~output", PolygonArray, queue_size=1)
00018 
00019         self.timer = rospy.Timer(rospy.Duration(1. / publish_rate), self.publish)
00020 
00021     def parse_params(self, param, frame_id):
00022         # validate params
00023         assert isinstance(param, list), "polygons must be list"
00024 
00025         has_label = any("label" in p for p in param)
00026         has_likelihood = any("likelihood" in p for p in param)
00027 
00028         for polygon in param:
00029             assert "points" in polygon and isinstance(polygon["points"], list),\
00030                    "each polygon must have at least 1 'points'"
00031             for point in polygon["points"]:
00032                 assert len(point) == 3,\
00033                        "element of 'points' must be list of 3 numbers"
00034             if has_label:
00035                 assert "label" in polygon, "missing 'label'"
00036             if has_likelihood:
00037                 assert "likelihood" in polygon, "missing 'likelihood'"
00038 
00039         # parse params
00040         msg = PolygonArray()
00041         msg.header.frame_id = frame_id
00042         for polygon in param:
00043             ps = PolygonStamped()
00044             ps.header.frame_id = frame_id
00045             ps.polygon.points = [Point32(*v) for v in polygon["points"]]
00046             msg.polygons.append(ps)
00047             if has_label:
00048                 msg.labels.append(polygon["label"])
00049             if has_likelihood:
00050                 msg.likelihood.append(polygon["likelihood"])
00051 
00052         return msg
00053 
00054     def publish(self, event):
00055         # update timestamp
00056         self.msg.header.stamp = event.current_real
00057         for p in self.msg.polygons:
00058             p.header.stamp = event.current_real
00059 
00060         self.pub_polygons.publish(self.msg)
00061 
00062 if __name__ == '__main__':
00063     rospy.init_node("polygon_array_publisher")
00064     p = PolygonArrayPublisher()
00065     rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Tue Jul 2 2019 19:40:37