image_time_diff.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 #
4 import cv2
5 
6 import rospy
7 import cv_bridge
8 import message_filters
9 import dynamic_reconfigure.server
10 
11 from std_msgs.msg import Header
12 from sensor_msgs.msg import Image
13 from jsk_recognition_msgs.msg import ImageDifferenceValue
14 from jsk_perception.cfg import ImageTimeDiffConfig
15 
16 
18  """Publish difference between stored image and current input one
19 
20  Publications:
21  * ~output/diff: diff per pixel: 0 - 1
22  * ~output/diff_image: diff image
23 
24  Subscriptions:
25  * ~input/hue: input hue image
26  * ~input/saturation: input saturation image
27  * ~start: msg to store image and start comparing
28  * ~stop: msg to relase image and stop comparing
29 
30  """
31  def __init__(self):
32  dynamic_reconfigure.server.Server(ImageTimeDiffConfig,
33  self._cb_dyn_reconfig)
34  sub_hue = message_filters.Subscriber('~input/hue', Image)
35  sub_saturation = message_filters.Subscriber('~input/saturation', Image)
36  ts = message_filters.TimeSynchronizer([sub_hue, sub_saturation], 10)
37  ts.registerCallback(self._cb_input)
38  rospy.Subscriber('~start', Header, self._cb_start)
39  rospy.Subscriber('~stop', Header, self._cb_stop)
40  self.pub_stored = None
41  self.input = None
42 
43  def _cb_dyn_reconfig(self, config, level):
44  self.s_threshold = config['saturation_threshold']
45  return config
46 
47  def _cb_input(self, imgmsg_hue, imgmsg_saturation):
48  self.input = (imgmsg_hue, imgmsg_saturation)
49 
50  def _cb_start(self, header):
51  while ( (self.input is None) or
52  (header.stamp > self.input[0].header.stamp) ):
53  rospy.sleep(0.1)
54  imgmsg_hue, imgmsg_saturation = self.input
55  bridge = cv_bridge.CvBridge()
56  hue = bridge.imgmsg_to_cv2(imgmsg_hue, desired_encoding='mono8')
57  saturation = bridge.imgmsg_to_cv2(imgmsg_saturation,
58  desired_encoding='mono8')
59  pub_diff = rospy.Publisher('~output/diff',
60  ImageDifferenceValue,
61  queue_size=1)
62  pub_diff_img = rospy.Publisher('~output/diff_image',
63  Image,
64  queue_size=1)
65  self.pub_stored = (hue, saturation, pub_diff, pub_diff_img)
66 
67  def _cb_stop(self, header):
68  while header.stamp > self.input[0].header.stamp:
69  rospy.sleep(0.1)
70  self.pub_stored = None
71 
72  def spin_once(self):
73  if (self.input is None) or (self.pub_stored is None):
74  return
75  imgmsg_hue, imgmsg_saturation = self.input
76  pub_stored = self.pub_stored
77  bridge = cv_bridge.CvBridge()
78  hue = bridge.imgmsg_to_cv2(imgmsg_hue, desired_encoding='mono8')
79  saturation = bridge.imgmsg_to_cv2(imgmsg_saturation,
80  desired_encoding='mono8')
81  hue_stored, saturation_stored, pub_diff, pub_diff_img = pub_stored
82  # compute diff and publish the result
83  diff_img = cv2.absdiff(hue, hue_stored)
84  diff_msg = ImageDifferenceValue()
85  diff_msg.header.stamp = imgmsg_hue.header.stamp
86  mask = ( (saturation_stored > self.s_threshold) &
87  (saturation > self.s_threshold) )
88  mask = mask.reshape((mask.shape[0], mask.shape[1]))
89  filtered_diff_img = diff_img[mask]
90  diff_msg.difference = ( filtered_diff_img.sum()
91  / filtered_diff_img.size
92  / 255. )
93  pub_diff.publish(diff_msg)
94  diff_img_msg = bridge.cv2_to_imgmsg(diff_img, encoding='mono8')
95  diff_img_msg.header = diff_msg.header
96  pub_diff_img.publish(diff_img_msg)
97 
98  def spin(self):
99  rate = rospy.Rate(rospy.get_param('rate', 10))
100  while not rospy.is_shutdown():
101  self.spin_once()
102  try:
103  rate.sleep()
104  except rospy.ROSTimeMovedBackwardsException:
105  pass
106 
107 
108 if __name__ == '__main__':
109  rospy.init_node('image_time_diff')
110  image_time_diff = ImageTimeDiff()
111  image_time_diff.spin()
def _cb_input(self, imgmsg_hue, imgmsg_saturation)


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Mon May 3 2021 03:03:27