extract_top_polygon_likelihood.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy 
00004 from jsk_topic_tools import ConnectionBasedTransport
00005 from jsk_recognition_msgs.msg import PolygonArray, ModelCoefficientsArray
00006 from pcl_msgs.msg import ModelCoefficients
00007 from geometry_msgs.msg import PolygonStamped
00008 import message_filters
00009 from dynamic_reconfigure.server import Server
00010 from jsk_pcl_ros.cfg import ExtractTopPolygonLikelihoodConfig
00011 
00012 class ExtractTopPolygonLikelihood(ConnectionBasedTransport):
00013     def __init__(self):
00014         super(ExtractTopPolygonLikelihood, self).__init__()
00015         self._srv = Server(ExtractTopPolygonLikelihoodConfig, self.config_callback)
00016         self._pub = self.advertise("~output", PolygonArray, queue_size=1)
00017         self._pub_coef = self.advertise("~output/coefficients", ModelCoefficientsArray, queue_size=1)
00018     def config_callback(self, config, level):
00019         self._min_likelihood = config.min_likelihood
00020         return config
00021     def subscribe(self):
00022         self._sub = message_filters.Subscriber("~input", PolygonArray)
00023         self._sub_coef = message_filters.Subscriber("~input/coefficients", ModelCoefficientsArray)
00024         self._sync = message_filters.TimeSynchronizer([self._sub, self._sub_coef], 100)
00025         self._sync.registerCallback(self.callback)
00026     def unsubscribe(self):
00027         self._sub.sub.unregister()
00028         self._sub_coef.sub.unregister()
00029     def callback(self, msg, msg_coef):
00030         if len(msg.polygons) > 0:
00031             #self._pub.publish(msg.histograms[0])
00032             max_index = max(xrange(len(msg.polygons)), key=lambda i: msg.likelihood[i])
00033             res = PolygonArray()
00034             res.header = msg.header
00035             res.polygons = [msg.polygons[max_index]]
00036             res.likelihood = [msg.likelihood[max_index]]
00037             if msg.likelihood[max_index] < self._min_likelihood:
00038                 rospy.loginfo("Ignore result because of too small likelihood: {0} < {1}".format(
00039                     msg.likelihood[max_index],
00040                     self._min_likelihood))
00041                 return
00042             self._pub.publish(res)
00043             res_coef = ModelCoefficientsArray()
00044             res_coef.header = msg.header
00045             res_coef.coefficients = [msg_coef.coefficients[max_index]]
00046             self._pub_coef.publish(res_coef)
00047 
00048 
00049 if __name__ == "__main__":
00050     rospy.init_node("extract_top_polygon_likelihood")
00051     ex = ExtractTopPolygonLikelihood()
00052     rospy.spin()
00053     
00054 


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43