image_time_diff.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #
00004 import cv2
00005 
00006 import rospy
00007 import cv_bridge
00008 import message_filters
00009 import dynamic_reconfigure.server
00010 
00011 from std_msgs.msg import Header
00012 from sensor_msgs.msg import Image
00013 from jsk_recognition_msgs.msg import ImageDifferenceValue
00014 from jsk_perception.cfg import ImageTimeDiffConfig
00015 
00016 
00017 class ImageTimeDiff(object):
00018     """Publish difference between stored image and current input one
00019 
00020     Publications:
00021         * ~output/diff: diff per pixel: 0 - 1
00022         * ~output/diff_image: diff image
00023 
00024     Subscriptions:
00025         * ~input/hue: input hue image
00026         * ~input/saturation: input saturation image
00027         * ~start: msg to store image and start comparing
00028         * ~stop: msg to relase image and stop comparing
00029 
00030     """
00031     def __init__(self):
00032         dynamic_reconfigure.server.Server(ImageTimeDiffConfig,
00033                                           self._cb_dyn_reconfig)
00034         sub_hue = message_filters.Subscriber('~input/hue', Image)
00035         sub_saturation = message_filters.Subscriber('~input/saturation', Image)
00036         ts = message_filters.TimeSynchronizer([sub_hue, sub_saturation], 10)
00037         ts.registerCallback(self._cb_input)
00038         rospy.Subscriber('~start', Header, self._cb_start)
00039         rospy.Subscriber('~stop', Header, self._cb_stop)
00040         self.pub_stored = None
00041         self.input = None
00042 
00043     def _cb_dyn_reconfig(self, config, level):
00044         self.s_threshold = config['saturation_threshold']
00045         return config
00046 
00047     def _cb_input(self, imgmsg_hue, imgmsg_saturation):
00048         self.input = (imgmsg_hue, imgmsg_saturation)
00049 
00050     def _cb_start(self, header):
00051         while ( (self.input is None) or
00052                 (header.stamp > self.input[0].header.stamp) ):
00053             rospy.sleep(0.1)
00054         imgmsg_hue, imgmsg_saturation = self.input
00055         bridge = cv_bridge.CvBridge()
00056         hue = bridge.imgmsg_to_cv2(imgmsg_hue, desired_encoding='mono8')
00057         saturation = bridge.imgmsg_to_cv2(imgmsg_saturation,
00058                                           desired_encoding='mono8')
00059         pub_diff = rospy.Publisher('~output/diff',
00060                                    ImageDifferenceValue,
00061                                    queue_size=1)
00062         pub_diff_img = rospy.Publisher('~output/diff_image',
00063                                        Image,
00064                                        queue_size=1)
00065         self.pub_stored = (hue, saturation, pub_diff, pub_diff_img)
00066 
00067     def _cb_stop(self, header):
00068         while header.stamp > self.imgmsg.header.stamp:
00069             rospy.sleep(0.1)
00070         self.pub_stored = None
00071 
00072     def spin_once(self):
00073         if (self.input is None) or (self.pub_stored is None):
00074             return
00075         imgmsg_hue, imgmsg_saturation = self.input
00076         pub_stored = self.pub_stored
00077         bridge = cv_bridge.CvBridge()
00078         hue = bridge.imgmsg_to_cv2(imgmsg_hue, desired_encoding='mono8')
00079         saturation = bridge.imgmsg_to_cv2(imgmsg_saturation,
00080                                           desired_encoding='mono8')
00081         hue_stored, saturation_stored, pub_diff, pub_diff_img = pub_stored
00082         # compute diff and publish the result
00083         diff_img = cv2.absdiff(hue, hue_stored)
00084         diff_msg = ImageDifferenceValue()
00085         diff_msg.header.stamp = imgmsg_hue.header.stamp
00086         mask = ( (saturation_stored > self.s_threshold) &
00087                  (saturation > self.s_threshold) )
00088         mask = mask.reshape((mask.shape[0], mask.shape[1]))
00089         filtered_diff_img = diff_img[mask]
00090         diff_msg.difference = ( filtered_diff_img.sum()
00091                                 / filtered_diff_img.size
00092                                 / 255. )
00093         pub_diff.publish(diff_msg)
00094         pub_diff_img.publish(bridge.cv2_to_imgmsg(diff_img, encoding='mono8'))
00095 
00096     def spin(self):
00097         rate = rospy.Rate(rospy.get_param('rate', 10))
00098         while not rospy.is_shutdown():
00099             self.spin_once()
00100             rate.sleep()
00101 
00102 
00103 if __name__ == '__main__':
00104     rospy.init_node('image_time_diff')
00105     image_time_diff = ImageTimeDiff()
00106     image_time_diff.spin()


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Wed Sep 16 2015 04:36:15