#!/usr/bin/env python
import numpy as np
import cv2
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

bridge = CvBridge()
hog = cv2.HOGDescriptor()

def inside(r, q):
    rx, ry, rw, rh = r
    qx, qy, qw, qh = q
    return rx > qx and ry > qy and rx + rw < qx + qw and ry + rh < qy + qh


def draw_detections(img, rects, thickness = 1):
    for x, y, w, h in rects:
        # the HOG detector returns slightly larger rectangles than the real objects.
        # so we slightly shrink the rectangles to get a nicer output.
        pad_w, pad_h = int(0.15*w), int(0.05*h)
        cv2.rectangle(img, (x+pad_w, y+pad_h), (x+w-pad_w, y+h-pad_h), (0, 255, 0), thickness)

def callback(data):
    frame = bridge.imgmsg_to_cv2(data, "bgr8")
    found,w=hog.detectMultiScale(frame, winStride=(8,8), padding=(32,32), scale=1.05)
    draw_detections(frame,found)
    cv2.imshow('feed',frame)
    cv2.waitKey(3)
    #ch = 0xFF & cv2.waitKey(1)
    #if ch == 27:
        #break
    #cv2.destroyAllWindows()

if __name__ == '__main__':
    hog.setSVMDetector( cv2.HOGDescriptor_getDefaultPeopleDetector() )
    #cap=cv2.VideoCapture('video3.mp4')
    try:
       rospy.init_node('peopledetector_node', anonymous=False)
       rospy.Subscriber('/image_raw', Image, callback)
       rospy.spin()
    except rospy.ROSInterruptException:
        pass
