ros_smile_detector.py

#!/usr/bin/python

"""
This code is part of ros package that subscribes to an image topic
and detects smiles found in the image. Results are then republished to a ros
image topic.

This node is an example of how I prefer to write my ROS nodes, as a single
class with a run function.

Written by Sophie Li, 2017
http://blog.justsophie.com/smile-detection-using-opencv-designing-ros-node/
"""

import rospkg
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image

import cv2
import numpy as np
from copy import deepcopy

class FaceFinder:
    def __init__(self):
        rospy.init_node("face_finder_node", anonymous=True)
        self.face_status_pub = rospy.Publisher('/face_finder/status', String, queue_size = 10)
        self.face_img_pub = rospy.Publisher("/face_finder/rgb_image", Image, queue_size = 10)

        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("IMAGE_TOPIC_HERE", Image, self.image_callback)
        self.frame = None   # Initialize image frame to None

        ### Load haar cascades using relative pathing
        ### You'll have to run this node in the folder where the .xml files are located
        self.face_cascade = cv2.CascadeClassifier('haarcascade_frontalface_alt.xml')    #face xml
        self.mouth_cascade = cv2.CascadeClassifier('haarcascade_smile.xml')             #smile xml

        ### Or, use rospkg to load the .xml files from anywhere in your system depending on your ros package name
        # r = rospkg.RosPack()
        # PACKAGE_PATH = r.get_path()
        # self.face_cascade = cv2.CascadeClassifier(PACKAGE_PATH + 'haarcascade_frontalface_alt.xml')
        # self.mouth_cascade = cv2.CascadeClassifier(PACKAGE_PATH + 'haarcascade_smile.xml')

    def image_callback(self, data):
        try:
            # Convert the rosmsg received to a cv2 numpy array
            self.frame = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

    def find_smile(self):
        #Check if frame data has been recieved
        if self.frame == None:
            return

        gray_img = cv2.cvtColor(self.frame, cv2.COLOR_BGR2GRAY)
        faces = self.face_cascade.detectMultiScale(gray_img, scaleFactor=1.2, minSize=(20, 20))
        result_frame = deepcopy(self.frame)

        for (x, y, w, h) in faces:
            # Draw bounding box around face
            cv2.rectangle(result_frame, (x, y), (x+w, y+h), (0, 0, 255), 2)

            # Isolate face ROI (region of interest)
            roi_gray = gray[y:y+h, x:x+w]
            roi_color = result_frame[y:y+h, x:x+w]

            # Smile detection on face ROI
            smiles = self.mouth_cascade.detectMultiScale(roi_gray, scaleFactor=1.7, minNeighbors=20,              minSize=(10,10), flags=cv2.cv.CV_HAAR_SCALE_IMAGE)

            # Draw the detected smiles
            for (s_x, s_y, s_w, s_h) in smiles:
                cv2.rectangle(roi_color, (s_x, s_y),(s_x + s_w, s_y + s_h), (255 , 0, 0), 1)

        # Visualize results
        cv2.imshow("Smiles detected", result_frame)
        c = cv2.waitKey(1)

        # Publish results
        self.face_status_pub.publish("Found {} faces, {} were smiling".format(len(faces), len(mouth)))
        try:
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(result_image, "bgr8"))
        except CvBridgeError as e:
            print(e)

    def run(self):
        # Set detector rate to 10hz
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            self.find_smile()   # Run smile detector code once
            r.sleep()           # Sleep until time is up for next cycle to hit 10hz

if __name__ == "__main__":
    face_finder = FaceFinder()
    face_finder.run()