face_detection.py

#!/usr/bin/env python

import rospy
import sys
import cv2
import cv2 as cv
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import String
import numpy as np
font = cv2.FONT_HERSHEY_SIMPLEX


class cvBridgeDemo():
    def __init__(self,face_cascade):
        self.faces = None
        self.face_cascade = face_cascade

        self.node_name = "face_detection"
        #Initialize the ros node
        rospy.init_node(self.node_name)

        # What we do during shutdown
        rospy.on_shutdown(self.exit)

        # Create the cv_bridge object
        self.bridge = CvBridge()

        # Subscribe to the camera image and depth topics and set
        # the appropriate callbacks
        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.image_RGB_prepro)
        #self.depth_sub = rospy.Subscriber("/camera/depth_registered/image_raw", Image, self.image_detph_Prepro)
        self.depth_sub = rospy.Subscriber("/camera/depth/image_raw", Image, self.image_detph_Prepro)

        #rosrun dynamic_reconfigure reconfigure_gui

        #Callback executed when the timer timeout
        rospy.Timer(rospy.Duration(0.01),self.show_windows)

        rospy.loginfo("Waiting for image topics...")


    def show_windows(self,event):
        try:
            cv2.imshow("Processed_Image",self.display_image)
            cv2.imshow("Depth_Image",self.depth_display_image)

            cv2.waitKey(3)

        except:
            pass


    def image_detph_Prepro(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        try:
            # The depth image is a single-channel float32 image
            depth_image = self.bridge.imgmsg_to_cv2(ros_image, "32FC1")
        except CvBridgeError, e:
            print e
            pass

        # Convert the depth image to a Numpy array since most cv2 functions
        # require Numpy arrays.
        depth_array = np.array(depth_image, dtype=np.float32)

        self.depth_array = np.array(depth_image, dtype=np.float32)


        # Normalize the depth image to fall between 0 (black) and 1 (white)
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)

        # Process the depth image
        self.depth_display_image = self.check_Faces_depth(depth_array)

    def image_RGB_prepro(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        try:
            self.frame = self.bridge.imgmsg_to_cv2(ros_image, "bgr8")
        except CvBridgeError, e:
            print e
        pass

        # Convert the image to a Numpy array since most cv2 functions
        # require Numpy arrays.
        frame = np.array(self.frame, dtype=np.uint8)

        # Process the frame using the face_detection() function
        self.display_image = self.face_detection(frame)


    def face_detection(self, frame):

        # Convert to greyscale
        grey = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)

        # Detect faces
        faces = self.face_cascade.detectMultiScale(grey, 1.1, 4)

        self.faces = faces

        i = 1
        for (x, y, w, h) in faces:
            # Center coordinates for distance
            X_coord = int(x+int(w/2))
            Y_coord = int(y+int(h/2))
            # Draw rectangle around the faces
            cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2)
            cv2.putText(frame,'Person: '+str(i) ,(x,y-45), font, 0.5,(255,255,255),1)
            cv2.putText(frame,'distance: ',(x,y-30), font, 0.5,(255,255,255),1)

            #Get the distance to the faces
            distance = str(round(self.depth_array[X_coord][Y_coord] / 1000,2))
            #print distance
            cv2.putText(frame,distance+'m',(x,y-10), font, 0.5,(255,255,255),1)

            #X mark on the head
            cv2.putText(frame,"o",(X_coord,Y_coord), font, 0.3,(255,255,255),1)

            #cv2.putText(frame,"X",(x+10,y+20), font, 1,(255,255,255),1)
            i = i+1
        i = 0


        return frame


    def check_Faces_depth(self, frame):
        i = 1
        correction = -10 #Correction for death image and RGB image
        try:
            faces = self.faces
            for (x, y, w, h) in self.faces:

                # Center coordinates for distance
                X_coord = int((correction/2)+x+int(w/2))
                Y_coord = int((correction/2)+y+int(h/2))

                cv2.rectangle(frame, (x+correction, y+correction), (x+w, y+h), (255, 0, 0), 2)
                cv2.putText(frame,'Distance: ',(x,y-30), font, 0.5,(255,255,255),1)

                #Get the distance to the faces
                distance = str(round(self.depth_array[X_coord][Y_coord]/ 1000,2))
                cv2.putText(frame,distance+'m',(x,y-10), font, 0.5,(255,255,255),1)

                #print distance
                # X Mark on the head
                cv2.putText(frame,"o",(X_coord,Y_coord), font, 0.3,(255,255,255),1)
                i = i+1
            i=0
        except:
            frame = frame

        return frame


    def exit(self):
        print "Shutting down vision node."
        cv2.destroyAllWindows()

def main(args):
    # Get the face_cascade load to the class
    face_cascade = cv2.CascadeClassifier('/home/emmanuelc/catkin_ws/src/opencv_face_detection/scripts/haarcascade_frontalface_default.xml')

    try:
        cvBridgeDemo(face_cascade)
        rospy.spin()

    except KeyboardInterrupt:
        print "Shutting down vision node."
        cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)