#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cv2
import numpy
from hlf_module import hlf_define
from std_msgs.msg import String
import matplotlib.pyplot
as
plot
import xml.dom.minidom
import pylab
import rospy
import time
cap = cv2.VideoCapture(0)
cap.set(3,640) #设置分辨率
cap.set(4,480)
fps =cap.get(cv2.CAP_PROP_FPS) #获取视频帧数
face_casade = cv2.CascadeClassifier(
'/opt/ros/kinetic/share/OpenCV-3.2.0-dev/haarcascades/haarcascade_frontalface_default.xml'
)
Node_name=
'neck'
#
print
cap.isOpened()
class
Detect_face():
def __init__(self):
''
'定义节点Node_name(全局变量,而非具体名称)'
''
self.err_pub=hlf_define.err_publisher()#错误消息发布者
rospy.init_node(Node_name,anonymous=True)
self.neck_puber=rospy.Publisher(hlf_define.TOPIC_ACTION_NECK,String,queue_size=10)
time.sleep(0.5)
def head_motor_value(self):#解析xml文件 获取舵机的范围值
dom = xml.dom.minidom.parse(
'/home/sb/catkin_ws/src/hlf_robot/scripts/hlf_action/head_value.xml'
)
#得到文档元素对象
root = dom.documentElement
itemlist = root.getElementsByTagName(
'login'
)
item = itemlist[0]
max_value=item.getAttribute(
"max"
)
min_value=item.getAttribute(
"min"
)
return
max_value,min_value
def detect_face(self):
# get a frame
#frame=cv2.imread(
'/home/sb/桌面/timg.jpeg'
)
ret, frame = cap.read()
gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)#转成灰度图
#frame=cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
# show a frame
cv2.imshow(
"capture"
, gray)
faces = face_casade.detectMultiScale(gray,1.2,5) #检测人脸
#
print
len(faces)
if
len(faces)>0:#判断是否检测到人脸
result = ()
max_face = 0
value_x=0
for
(x,y,w,h) in faces:
if
(w*h > max_face): #检测最大人脸
max_face = w*h
result = (x,y,w,h)
# max_face.append(width*height)
x=result[0]
w=result[2]
z=value_x=value_x+x+w/2
return
z
else
:
return
1
if
__name__==
'__main__'
:
face=Detect_face()
motor_max,motor_min= face.head_motor_value()
x=[]
i=1
while
True:
try
:
z=face.detect_face()
if
z != 1:
x.append(z)
if
len(x)>(fps-1):
true_x = int(sum(x)/30)
if
(true_x>319):
motor_value=int(1500+(int(motor_max)-1500)*(true_x-319)/320)#转换成舵机值 头部向左转
face.neck_puber.publish(
'%s'
%motor_value)
elif (true_x<319):
motor_value=int(1500-(1500-int(motor_min))*(319-true_x)/320)
face.neck_puber.publish(
'%s'
%motor_value)
x=[]
else
:
if
i==fps:
face.neck_puber.publish(
'1500'
)
i=1
else
:
i +=1
print
(U
'未检测到人脸'
)
if
cv2.waitKey(1) & 0xFF == ord(
'q'
):
break
except Exception,e:
print
e
cap.release()
cv2.destroyAllWindows()