msgsCallback.py

#!/usr/bin/env python3
# ROS python API
import rospy
import numpy as np

# 3D point & Stamped Pose msgs
from geometry_msgs.msg import Point, PoseStamped

# oepncv
import cv2
from cv_bridge import CvBridge, CvBridgeError

# service
from mymsgs.srv import detect,detectResponse

net = cv2.dnn.readNet("../include/yolov3-tiny-head_final.weights", "../include/yolov3-tiny-head.cfg")
layer_names = net.getLayerNames()
output_layers = [layer_names[i[0] - 1] for i in net.getUnconnectedOutLayers()]

colors = [tuple(255 * np.random.rand(3)) for _ in range(10)]
scale = 0.00392
conf_threshold = 0.5
nms_threshold = 0.4
    
bridge = CvBridge()

def handle_detect_image(req):
	x, y, w, h , = 0,0,0,0
	try:
		# bgr8 is the pixel encoding -- 8 bits per color, organized as blue/green/red
		cv2_rgb = bridge.imgmsg_to_cv2(req.image, "bgr8")
		height,width,channel = cv2_rgb.shape
		image = cv2.resize(cv2_rgb, (416,416))
		blob = cv2.dnn.blobFromImage(image, scale, (416,416), (0, 0, 0), True)
		net.setInput(blob)
		outs=net.forward(output_layers)

		confidences = []
		boxes = []
		#centers = []
	   
		for out in outs:
		    for detection in out:
			    confidence = detection[5]
			    if confidence > 0.35:
			        #print detection
			        center_x = int(detection[0]*width)
			        center_y = int(detection[1]*height)
			        w = int(detection[2]*width)
			        h = int(detection[3]*height)
			        x = int(center_x - w/2)
			        y = int(center_y - h/2)
			        confidences.append(float(confidence))
			        boxes.append([x, y, w, h])
			        #centers.append([center_x,center_y])

		# apply non-max suppression
		indices = cv2.dnn.NMSBoxes(boxes, confidences, conf_threshold, nms_threshold)
		# go through the detections remaining after nms and draw bounding box
		for i in indices:
		    i = i[0]
		    x,y,w,h = boxes[i]
		    print("box",x,y,w,h)
		    #cv2.rectangle(cv2_rgb,(x,y),(x+w,y+h),(255,0,0),2)
		    #cv2.putText(cv2_rgb,str(round(confidence,2)),(x,y-10), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 0), 2)
		    #cv2.imshow("rgb_image_view", cv2_rgb)
		    #cv2.waitKey(1) 
		    
	except CvBridgeError as e:
		# all print statements should use a rospy.log_ form, don't print!
		rospy.loginfo("Conversion failed")
	print("Returning")
	return detectResponse(x,y,w,h)

def detect_image():
    rospy.init_node('detect_image_server')
    s = rospy.Service('detect_image', detect, handle_detect_image)
    print("Ready to detect image.")
    rospy.spin()

if __name__ == "__main__":
    detect_image()