2011/ros11: face_detect_node.py

File face_detect_node.py, 3.1 KB (added by cteo, 7 years ago)

face detection example code (python)

Line 
1#!/usr/bin/env python
2
3import roslib
4roslib.load_manifest('umd_image_proc')
5import sys
6import rospy
7import cv
8from std_msgs.msg import String
9from sensor_msgs.msg import Image
10from cv_bridge import CvBridge, CvBridgeError
11
12class test_vision_node:
13
14    def __init__(self):
15        rospy.init_node('umd_cvFaceDetect')
16
17        """ Give the OpenCV display window a name. """
18        self.cv_window_name = "OpenCV Image"
19
20        """ Create the window and make it re-sizeable (second parameter = 0) """
21        cv.NamedWindow(self.cv_window_name, 0)
22
23        """ Create the cv_bridge object """
24        self.bridge = CvBridge()
25
26        """ Subscribe to the raw camera image topic """
27        self.image_sub = rospy.Subscriber("/quad/right1/image_raw", Image, self.callback)
28        #self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.callback)
29        self.image_pub = rospy.Publisher("/umd_image_proc/image_umd_cvFaceDetect", Image)
30       
31    def detect(self, image):
32       
33        image_size = cv.GetSize(image)
34       
35        # create grayscale version
36        grayscale = cv.CreateImage(image_size, 8, 1)
37        cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY)
38       
39        # create storage
40        storage = cv.CreateMemStorage(0)
41        #cv.ClearMemStorage(storage)
42       
43        # equalize histogram
44        cv.EqualizeHist(grayscale, grayscale)
45       
46        # detect objects
47        cascade = cv.Load('/opt/ros/diamondback/stacks/vision_opencv/opencv2/opencv/share/opencv/haarcascades/haarcascade_frontalface_alt.xml')         
48        faces = cv.HaarDetectObjects(grayscale, cascade, storage, 1.2, 2, 0, (50, 50))
49       
50        return faces;
51
52    def callback(self, data):
53        try:
54            """ Convert the raw image to OpenCV format """
55            cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
56        except CvBridgeError, e:
57          print e
58 
59       
60        """ Get the width and height of the image """
61        (width, height) = cv.GetSize(cv_image)
62        faces = self.detect(cv_image)
63        text_font = cv.InitFont(cv.CV_FONT_HERSHEY_DUPLEX, 2, 2)
64        if faces:
65                print 'face detected!'
66                cv.PutText(cv_image, "face detected!", (50, height / 2), text_font, cv.RGB(255, 255, 0))
67                print faces;
68                for i in faces:
69                        cv.Rectangle(cv_image, (int(i[0][0]), int(i[0][1])),
70                                (int(i[0][0] + i[0][2]), int(i[0][1] + i[0][3])),
71                                cv.RGB(0, 255, 0), 3, 8, 0)
72
73        """ Overlay some text onto the image display """
74        #text_font = cv.InitFont(cv.CV_FONT_HERSHEY_DUPLEX, 2, 2)
75        #cv.PutText(cv_image, "OpenCV Image", (50, height / 2), text_font, cv.RGB(255, 255, 0))
76 
77        """ Refresh the image on the screen """
78        cv.ShowImage(self.cv_window_name, cv_image)
79        cv.WaitKey(3)
80
81        try:
82                self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8"))
83        except CvBridgeError, e:
84                print e
85
86       
87   
88
89
90def main(args):
91      vn = test_vision_node()
92      try:
93        rospy.spin()
94      except KeyboardInterrupt:
95        print "Shutting down vison node."
96      cv.DestroyAllWindows()
97
98if __name__ == '__main__':
99    main(sys.argv)