9 import dynamic_reconfigure.server
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
18 """Publish difference between stored image and current input one 21 * ~output/diff: diff per pixel: 0 - 1 22 * ~output/diff_image: diff image 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 32 dynamic_reconfigure.server.Server(ImageTimeDiffConfig,
38 rospy.Subscriber(
'~start', Header, self.
_cb_start)
39 rospy.Subscriber(
'~stop', Header, self.
_cb_stop)
48 self.
input = (imgmsg_hue, imgmsg_saturation)
51 while ( (self.
input is None)
or 52 (header.stamp > self.
input[0].header.stamp) ):
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',
62 pub_diff_img = rospy.Publisher(
'~output/diff_image',
65 self.
pub_stored = (hue, saturation, pub_diff, pub_diff_img)
68 while header.stamp > self.
input[0].header.stamp:
75 imgmsg_hue, imgmsg_saturation = self.
input 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
83 diff_img = cv2.absdiff(hue, hue_stored)
84 diff_msg = ImageDifferenceValue()
85 diff_msg.header.stamp = imgmsg_hue.header.stamp
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
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)
99 rate = rospy.Rate(rospy.get_param(
'rate', 10))
100 while not rospy.is_shutdown():
104 except rospy.ROSTimeMovedBackwardsException:
108 if __name__ ==
'__main__':
109 rospy.init_node(
'image_time_diff')
111 image_time_diff.spin()
def _cb_input(self, imgmsg_hue, imgmsg_saturation)
def _cb_dyn_reconfig(self, config, level)
def _cb_start(self, header)
def _cb_stop(self, header)