diff --git a/CMakeLists.txt b/CMakeLists.txt
index 51a2e74..60da57f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,6 +7,8 @@ project(simple_face_tracker)
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
+ geometry_msgs
+ message_generation
)
## System dependencies are found with CMake's conventions
@@ -43,11 +45,11 @@ find_package(catkin REQUIRED COMPONENTS
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
-# add_message_files(
-# FILES
-# Message1.msg
+ add_message_files(
+ FILES
+ targets.msg
# Message2.msg
-# )
+ )
## Generate services in the 'srv' folder
# add_service_files(
@@ -64,10 +66,10 @@ find_package(catkin REQUIRED COMPONENTS
# )
## Generate added messages and services with any dependencies listed here
-# generate_messages(
-# DEPENDENCIES
-# std_msgs
-# )
+ generate_messages(
+ DEPENDENCIES
+ geometry_msgs
+ )
###################################
## catkin specific configuration ##
@@ -81,7 +83,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES simple-face-tracker
-# CATKIN_DEPENDS rospy std_msgs
+ CATKIN_DEPENDS rospy std_msgs geometry_msgs message_runtime
# DEPENDS system_lib
)
diff --git a/face_detect.py b/face_detect.py
index 113300a..9703603 100755
--- a/face_detect.py
+++ b/face_detect.py
@@ -7,120 +7,111 @@
# http://japskua.wordpress.com/2010/08/04/detecting-eyes-with-python-opencv
#----------------------------------------------------------------------------
-#import roslib
-#roslib.load_manifest
-
import cv
-import time
-#import Image
import rospy
-
import sys
from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+from simple_face_tracker.msg import targets
+
from cv_bridge import CvBridge, CvBridgeError
-from std_msgs.msg import String
-from std_msgs.msg import UInt16MultiArray
-class face_detect:
- def __init__(self):
- self.haarbase = '/opt/ros/hydro/share/OpenCV/'
- self.faceCascade = cv.Load(self.haarbase + "haarcascades/haarcascade_frontalface_alt.xml")
- self.pub = rospy.Publisher('facedetect', UInt16MultiArray, queue_size=10)
- #cv.NamedWindow("Image window", 1)
- self.bridge = CvBridge()
- self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback)
-
- def callback(self,data):
- try:
- cv_image = self.bridge.imgmsg_to_cv(data,"bgr8")
- except CvBridgeError, e:
- print e
-
- (cols,rows) = cv.GetSize(cv_image)
- img = self.DetectFace(cv_image,self.faceCascade)
- cv.ShowImage("ImageShow", img)
- cv.WaitKey(10)
+class FaceDetect:
+ NODE_NAME = 'face_detect'
+
+ def __init__(self):
+ self.haarbase = '/opt/ros/hydro/share/OpenCV/' #'/usr/share/opencv/'
+ self.faceCascade = cv.Load(self.haarbase + "haarcascades/haarcascade_frontalface_alt.xml")
+ self.pub = rospy.Publisher('facedetect', targets, queue_size=10)
+ cv.NamedWindow("ImageShow", 1)
+ self.bridge = CvBridge()
+ self.image_sub = rospy.Subscriber("/cv_camera/image_raw",Image,self.callback)
+
+ def callback(self,data):
+ try:
+ cv_image = self.bridge.imgmsg_to_cv(data,"bgr8")#imgmsg_to_cv2
+ except CvBridgeError, e:
+ print e
+
+ (cols,rows) = cv.GetSize(cv_image)
+ img = self.DetectFace(cv_image,self.faceCascade)
+ cv.ShowImage("ImageShow", img)
+ cv.WaitKey(10)
#rospy.init_node('face_locations', anonymous=True)
#r = rospy.Rate(10) # 10hz
- def DetectFace(self,image, faceCascade):
-
- min_size = (20,20)
- image_scale = 2
- haar_scale = 1.1
- min_neighbors = 3
- haar_flags = 0
-
- # Allocate the temporary images
- grayscale = cv.CreateImage((image.width, image.height), 8, 1)
- smallImage = cv.CreateImage(
- (
- cv.Round(image.width / image_scale),
- cv.Round(image.height / image_scale)
- ), 8 ,1)
-
- # Convert color input image to grayscale
- cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY)
-
- # Scale input image for faster processing
- cv.Resize(grayscale, smallImage, cv.CV_INTER_LINEAR)
-
- # Equalize the histogram
- cv.EqualizeHist(smallImage, smallImage)
-
- # Detect the faces
- faces = cv.HaarDetectObjects(
- smallImage, faceCascade, cv.CreateMemStorage(0),
- haar_scale, min_neighbors, haar_flags, min_size
- )
-
- # If faces are found
- if faces:
- payload = []
- for ((x, y, w, h), n) in faces:
- # the input to cv.HaarDetectObjects was resized, so scale the
- # bounding box of each face and convert it to two CvPoints
- pt1 = (int(x * image_scale), int(y * image_scale))
- pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
- cv.Rectangle(image, pt1, pt2, cv.RGB(255, 0, 0), 5, 8, 0)
- payload.append(x + w/2)
- payload.append(y + h/2)
+ def DetectFace(self,image, faceCascade):
+
+ min_size = (20,20)
+ image_scale = 2
+ haar_scale = 1.1
+ min_neighbors = 3
+ haar_flags = 0
+
+ # Allocate the temporary images
+ grayscale = cv.CreateImage((image.width, image.height), 8, 1)
+ smallImage = cv.CreateImage(
+ (
+ cv.Round(image.width / image_scale),
+ cv.Round(image.height / image_scale)
+ ), 8 ,1)
+
+ # Convert color input image to grayscale
+ cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY)
+
+ # Scale input image for faster processing
+ cv.Resize(grayscale, smallImage, cv.CV_INTER_LINEAR)
+
+ # Equalize the histogram
+ cv.EqualizeHist(smallImage, smallImage)
+
+ # Detect the faces
+ faces = cv.HaarDetectObjects(
+ smallImage, faceCascade, cv.CreateMemStorage(0),
+ haar_scale, min_neighbors, haar_flags, min_size
+ )
+
+ # If faces are found
+ if faces:
+ payload=targets()
+ for ((x, y, w, h), n) in faces:
+ # the input to cv.HaarDetectObjects was resized, so scale the
+ # bounding box of each face and convert it to two CvPoints
+ pt1 = (int(x * image_scale), int(y * image_scale))
+ pt2 = (int((x + w) * image_scale), int((y + h) * image_scale))
+ cv.Rectangle(image, pt1, pt2, cv.RGB(255, 0, 0), 5, 8, 0)
+ fpt=Point()
+ fpt.x=(float(x) + float(w)/2.0)/float(smallImage.width)
+ fpt.y=(float(y) + float(h)/2.0)/float(smallImage.height)
+ payload.positions.append(fpt)
- msg = UInt16MultiArray(None, payload)
+ msg = payload
- rospy.loginfo(msg)
- self.pub.publish(msg)
-
- return image
+ rospy.loginfo(msg)
+ self.pub.publish(msg)
+
+ return image
#----------
# M A I N
#----------
def main(args):
- ic = face_detect()
- rospy.init_node('face_detect',anonymous=True)
- try:
- rospy.spin()
- except KeyboardInterupt:
- print "shutting down"
- cv.DestroyAllWindows()
-
-if __name__=='__main__':
- main(sys.argv)
-##haarbase = '/opt/ros/hydro/share/OpenCV/'
-##capture = cv.CaptureFromCAM(0)
-#capture = cv.CaptureFromFile("test.avi")
-
-#faceCascade = cv.Load("haarcascades/haarcascade_frontalface_default.xml")
-#faceCascade = cv.Load("haarcascades/haarcascade_frontalface_alt2.xml")
-##faceCascade = cv.Load(haarbase + "haarcascades/haarcascade_frontalface_alt.xml")
-#faceCascade = cv.Load("haarcascades/haarcascade_frontalface_alt_tree.xml")
+ rospy.loginfo("Starting {0}...".format(FaceDetect.NODE_NAME))
-
-##while (cv.WaitKey(15)==-1):
-# img = cv.QueryFrame(capture)
-# image = DetectFace(img, faceCascade)
-# cv.ShowImage("face detection test", image)
-
-##cv.ReleaseCapture(capture)
+ ic = FaceDetect()
+
+ rospy.init_node('face_detect', anonymous=True)
+
+ rospy.loginfo("{0} started, listening for image input on topic /cv_camera/image_raw, publishing on topic facedetect".format(FaceDetect.NODE_NAME))
+ try:
+ rospy.spin()
+ except KeyboardInterrupt:
+ print "shutting down"
+ rospy.loginfo("Stopping {0}...".format(FaceDetect.NODE_NAME))
+ rospy.loginfo("{0} stopped.".format(FaceDetect.NODE_NAME))
+
+ cv.DestroyAllWindows()
+
+if __name__=='__main__':
+ main(sys.argv)
diff --git a/msg/targets.msg b/msg/targets.msg
new file mode 100644
index 0000000..d204f7a
--- /dev/null
+++ b/msg/targets.msg
@@ -0,0 +1 @@
+geometry_msgs/Point[] positions
diff --git a/package.xml b/package.xml
index 7661c0f..b298f64 100644
--- a/package.xml
+++ b/package.xml
@@ -42,9 +42,12 @@
catkin
rospy
std_msgs
+ geometry_msgs
+ message_generation
rospy
std_msgs
-
+ message_runtime
+ geometry_msgs