image_cluster_indices_decomposer.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 import cv_bridge
00005 from jsk_recognition_msgs.msg import ClusterPointIndices
00006 from jsk_topic_tools import ConnectionBasedTransport
00007 import message_filters
00008 import rospy
00009 from sensor_msgs.msg import Image
00010 
00011 
00012 class ImageClusterIndicesDecomposer(ConnectionBasedTransport):
00013     def __init__(self):
00014         super(ImageClusterIndicesDecomposer, self).__init__()
00015         self._pub = self.advertise('~output', Image, queue_size=1)
00016 
00017     def subscribe(self):
00018         self._sub = message_filters.Subscriber('~input', Image)
00019         self._sub_cpi = message_filters.Subscriber('~input/cluster_indices',
00020                                                    ClusterPointIndices)
00021         use_async = rospy.get_param('~approximate_sync', False)
00022         queue_size = rospy.get_param('~queue_size', 100)
00023         subs = [self._sub, self._sub_cpi]
00024         if use_async:
00025             slop = rospy.get_param('~slop', 0.1)
00026             sync = message_filters.ApproximateTimeSynchronizer(
00027                 subs, queue_size, slop)
00028         else:
00029             sync = message_filters.TimeSynchronizer(subs, queue_size)
00030         sync.registerCallback(self._decompose)
00031 
00032     def unsubscribe(self):
00033         self._sub.unregister()
00034         self._sub_cpi.unregister()
00035 
00036     def _decompose(self, imgmsg, cpi_msg):
00037         bridge = cv_bridge.CvBridge()
00038         img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
00039 
00040         from jsk_recognition_utils import colorize_cluster_indices
00041         cluster_indices = [list(i_msg.indices)
00042                            for i_msg in cpi_msg.cluster_indices]
00043         out = colorize_cluster_indices(img, cluster_indices)
00044 
00045         out_msg = bridge.cv2_to_imgmsg(out, encoding='rgb8')
00046         out_msg.header = imgmsg.header
00047         self._pub.publish(out_msg)
00048 
00049 
00050 if __name__ == '__main__':
00051     rospy.init_node('image_cluster_indices_decomposer')
00052     app = ImageClusterIndicesDecomposer()
00053     rospy.spin()


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Sun Oct 8 2017 02:43:23