Go to the documentation of this file.00001
00002
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
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 diff_img_msg = bridge.cv2_to_imgmsg(diff_img, encoding='mono8')
00095 diff_img_msg.header = diff_msg.header
00096 pub_diff_img.publish(diff_img_msg)
00097
00098 def spin(self):
00099 rate = rospy.Rate(rospy.get_param('rate', 10))
00100 while not rospy.is_shutdown():
00101 self.spin_once()
00102 rate.sleep()
00103
00104
00105 if __name__ == '__main__':
00106 rospy.init_node('image_time_diff')
00107 image_time_diff = ImageTimeDiff()
00108 image_time_diff.spin()