def detectFaceFromImgSrvCallback(self, req):
#rospy.logdebug("FACE DETECTION isIMGFace:"+str(req.isImgFace))
img = req.img
try:
frame, label_r, top_r, left_r, bottom_r, right_r, detected_faces_list = self.process_img_face(
img, None, self.DETECTION_STATUS, req.isImgFace)
eList = Entity2DList()
entity2D_list = []
for face in detected_faces_list:
#publish boxes information
detected_face = Entity2D()
detected_face.header.frame_id = img.header.frame_id
detected_face.label = face.label
detected_face.score = face.distance
x0, y0 = self._processBoxCenter(face.top, face.right,
face.bottom,
face.left) #ruthrashnono
detected_face.pose.x = x0
detected_face.pose.y = y0
box = Box()
box.x = face.top
box.y = face.left
box.width = abs(face.left - face.right)
box.height = abs(face.top - face.bottom)
detected_face.bounding_box = box
entity2D_list.append(detected_face)
eList.entity2DList = entity2D_list
#-rospy.loginfo(str(eList))
return DetectFaceFromImgResponse(eList)
except:
rospy.loginfo("Service Detect face from Img end with error " +
str(sys.exc_info()[0]))
return False
def _publishOnlyFaceRosMsg(self, data, data_result, detected_faces_map):
face_list = []
eList = Entity2DList()
for (top, right, bottom, left) in detected_faces_map.values():
#rospy.loginfo("top: %s, right: %s, bottom: %s, left:%s",str(top), str(right), str(bottom), str(left))
#publish boxes information
detected_face = Entity2D()
detected_face.header.frame_id = data.header.frame_id
detected_face.label = self.LABEL_FACE
x0, y0 = self._processBoxCenter(left, top, right, bottom)
print(left, top, right, bottom)
print("sadfjhasjkdfhaskjf")
detected_face.pose.x = x0
detected_face.pose.y = y0
box = Box()
box.x = top
box.y = left
box.width = abs(left - right)
box.height = abs(top - bottom)
detected_face.bounding_box = box
face_list.append(detected_face)
#rospy.loginfo("msg sent:"+str(detected_face))
eList.entity2DList = face_list
self.pub_all_faces_detections_msg.publish(eList)
#publish image with detected faces if needed
if (self.publish_img):
msg_im = self._bridge.cv2_to_imgmsg(data_result, encoding="bgr8")
msg_im.header.frame_id = data.header.frame_id
self.pub_all_faces_detections_image.publish(msg_im)
def BoundingBoxes_to_Entity2DList(self, bounding_boxes, labels):
el = Entity2DList()
el.header = bounding_boxes.header
words = self.Trad()
print words
print " "
for bounding_box in bounding_boxes.bounding_boxes:
if bounding_box.Class in words.keys():
word = words[bounding_box.Class]
else:
word = 'There are no object found'
print word
if len(labels) == 0 or word in labels:
e = Entity2D()
e.label = word
#e.xxx = bounding_box.probability
e.pose.x = (bounding_box.xmin + bounding_box.xmax) / 2
e.pose.y = (bounding_box.ymin + bounding_box.ymax) / 2
e.bounding_box.x = bounding_box.xmin
e.bounding_box.y = bounding_box.ymin
e.bounding_box.width = (bounding_box.xmax - bounding_box.xmin)
e.bounding_box.height = (bounding_box.ymax - bounding_box.ymin)
el.entity2DList.append(e)
return el
def _publishRosMsg(self, data, data_result, detected_faces_list):
eList = Entity2DList()
entity2D_list = []
for face in detected_faces_list:
#rospy.loginfo("top: %s, right: %s, bottom: %s, left:%s",str(top), str(right), str(bottom), str(left))
#publish boxes information
detected_face = Entity2D()
#detected_face.header.frame_id=data.header.frame_id
#detected_face.header.stamp = rospy.get_rostime()
detected_face.label = face.label
x0, y0 = self._processBoxCenter(face.top, face.right, face.bottom,
face.left)
detected_face.pose.x = x0
detected_face.pose.y = y0 #ruthrashyesyes
box = Box()
box.x = face.top
box.y = face.left
box.width = abs(face.left - face.right)
box.height = abs(face.top - face.bottom)
detected_face.bounding_box = box
detected_face.score = round(face.distance * 100, 1)
entity2D_list.append(detected_face)
eList.header = data.header
eList.entity2DList = entity2D_list
self.pub_detections_msg.publish(eList)
if (self.publish_img):
msg_im = self._bridge.cv2_to_imgmsg(data_result, encoding="bgr8")
msg_im.header.frame_id = data.header.frame_id
self.pub_detections_image.publish(msg_im)
def worker(self, input_q, output_q, name_w):
while True:
try:
data = self.input_q.get(0.05)
if data != None:
data_result, label, top, left, bottom, right = self.process_img(
data, input_q, output_q, name_w)
if (label != 'NONE'):
detected_face = Entity2D()
detected_face.label = label
x0, y0 = self.processBoxCenter(left, top, right,
bottom)
detected_face.pose.x = x0
detected_face.pose.y = y0
self.pub_detections_msg.publish(detected_face)
if (self.publish_img):
msg_im = self._bridge.cv2_to_imgmsg(data_result,
encoding="bgr8")
self.pub_detections_image.publish(msg_im)
#output_q.put(data_result)
except Full as e:
rospy.logwarn(e)