bhatta.py

#!/usr/bin/env python
import math
import rospy
from jsk_pcl_ros.msg import ColorHistogram

rospy.init_node("bhatta_test")

prev_hist = None

def cb(msg):
    global prev_hist
    if prev_hist:
        current_hist = msg.histogram
        if len(current_hist) == len(prev_hist):
            # normalize
            current_hist = list(current_hist)
            prev_hist = list(prev_hist)
            current_hist_sum = sum(current_hist)
            for i in range(len(current_hist)):
                current_hist[i] = current_hist[i] / current_hist_sum
            prev_hist_sum = sum(prev_hist)
            for i in range(len(prev_hist)):
                prev_hist[i] = prev_hist[i] / prev_hist_sum
            d = sum([math.sqrt(a * b) 
                     for (a, b) in zip(current_hist, prev_hist)])
            print d
    prev_hist = msg.histogram
    
s = rospy.Subscriber("/usb_cam/image_raw/hue_histogram", ColorHistogram, cb)
rospy.spin()