Last active
April 5, 2018 03:48
-
-
Save srli/151e4d17b442e3b5d28bbe29db7c3e95 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment