modifications for using ros point vector msg for blender

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
 )
 

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)

msg/targets.msg

@@ -0,0 +1 @@
+geometry_msgs/Point[] positions

package.xml

@@ -42,9 +42,12 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
-
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>