speak_when_label_found.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from __future__ import division
00004 
00005 import os
00006 import shutil
00007 import tempfile
00008 
00009 import cv_bridge
00010 from jsk_gui_msgs.msg import SlackMessage
00011 import message_filters
00012 import rospy
00013 from sensor_msgs.msg import Image
00014 from sound_play.libsoundplay import SoundClient
00015 from std_msgs.msg import String
00016 
00017 
00018 class SpeakWhenLabelFound(object):
00019 
00020     def __init__(self):
00021         self.tmpdir = tempfile.mkdtemp()
00022 
00023         self.pub_str = rospy.Publisher('~output/string', String, queue_size=1)
00024         self.pub_slack = rospy.Publisher(
00025             '~output/slack_msg', SlackMessage, queue_size=1)
00026 
00027         self.sound = None
00028         if rospy.get_param('~sound', True):
00029             self.sound = SoundClient(blocking=True)
00030             rospy.sleep(1)
00031 
00032         # Get target label id and label_names
00033         self.label_names = rospy.get_param('~label_names')
00034         self.target_labels = rospy.get_param('~target_labels', None)
00035         if self.target_labels is None:
00036             self.target_labels = []
00037             target_label_names = rospy.get_param('~target_label_names')
00038             for lname in target_label_names:
00039                 self.target_labels.append(self.label_names.index(lname))
00040         self.min_label_region = rospy.get_param('~min_label_region', 0.1)
00041 
00042         self.sub_img = message_filters.Subscriber(
00043             '~input/image', Image, queue_size=1)
00044         self.sub_label = message_filters.Subscriber(
00045             '~input/label', Image, queue_size=1)
00046         queue_size = rospy.get_param('~queue_size', 100)
00047         if rospy.get_param('~approximate_sync', False):
00048             self.sync = message_filters.TimeSynchronizer(
00049                 [self.sub_img, self.sub_label], queue_size=queue_size)
00050         else:
00051             slop = rospy.get_param('~slop', 0.1)
00052             self.sync = message_filters.ApproximateTimeSynchronizer(
00053                 [self.sub_img, self.sub_label], queue_size=queue_size, slop=slop)
00054         self.sync.registerCallback(self.input_cb)
00055 
00056     def __del__(self):
00057         if os.path.exists(self.tmpdir):
00058             shutil.rmtree(self.tmpdir)
00059 
00060     def input_cb(self, imgmsg, label_msg):
00061         bridge = cv_bridge.CvBridge()
00062         label = bridge.imgmsg_to_cv2(label_msg)
00063         found_labels = []
00064         for lval in self.target_labels:
00065             label_region = (label == lval).sum() / label.size
00066             if label_region >= self.min_label_region:
00067                 found_labels.append(lval)
00068 
00069         if not found_labels:
00070             rospy.loginfo('No label is found')
00071             return
00072 
00073         text = ' and '.join(self.label_names[lval] for lval in found_labels)
00074         text += ' is found'
00075         rospy.loginfo(text)
00076         self.pub_str.publish(String(data=text))
00077         if self.sound:
00078             self.sound.say(text)
00079 
00080         if self.pub_slack.get_num_connections() > 0:
00081             slack_msg = SlackMessage(text=text)
00082             if imgmsg:
00083                 slack_msg.image = imgmsg
00084             self.pub_slack.publish(slack_msg)
00085 
00086 
00087 if __name__ == '__main__':
00088     rospy.init_node('speak_when_label_found')
00089     SpeakWhenLabelFound()
00090     rospy.spin()


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