polygon_array_to_polygon.py

Go to the documentation of this file.

#!/usr/bin/env python

from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

import dynamic_reconfigure.server
from geometry_msgs.msg import PolygonStamped
from jsk_recognition_msgs.msg import PolygonArray
from jsk_topic_tools import ConnectionBasedTransport
from jsk_topic_tools.log_utils import logerr_throttle
from jsk_recognition_utils.cfg import PolygonArrayToPolygonConfig
import rospy


class PolygonArrayToPolygon(ConnectionBasedTransport):

    def __init__(self):
        super(self.__class__, self).__init__()
        self.pub = self.advertise('~output', PolygonStamped, queue_size=1)
        dynamic_reconfigure.server.Server(PolygonArrayToPolygonConfig,
                                          self._config_callback)

    def subscribe(self):
        self.sub = rospy.Subscriber('~input', PolygonArray, self._convert)

    def unsubscribe(self):
        self.sub.unregister()

    def _config_callback(self, config, level):
        self.index = config.index
        return config

    def _convert(self, msg):
        polygon_msg = PolygonStamped()
        polygon_msg.header = msg.header
        if self.index < 0:
            return
        elif self.index < len(msg.polygons):
            polygon_msg = msg.polygons[self.index]
            self.pub.publish(polygon_msg)
        else:
            logerr_throttle(10,
                            'Invalid index {} is specified '
                            'for polygon array whose size is {}'
                            .format(self.index, len(msg.polygons)))


if __name__ == '__main__':
    rospy.init_node('polygon_array_to_polygon')
    PolygonArrayToPolygon()
    rospy.spin()