def main(args): global depth_frame, rgb_frame predictionArray = None centroidArray = None labels = None rgb_frame = None depth_frame = None # cv.NamedWindow('rgb_frame', cv.CV_WINDOW_NORMAL) # cv.MoveWindow('rgb_frame', 25, 75) while True: try: objectDetection() msgPub = rospy.Publisher("vis_imgCoord",IC, queue_size = 10) while rgb_frame != None: # cv2.imshow('rgb_frame',rgb_frame) # cv2.waitkey(0) predictionArray, centroidArray,labels = processImage(rgb_frame) msg = IC() depthArray = np.array(depth_frame, dtype=np.float32) depthDisplayImage = cv2.normalize(depthArray, depthArray, 0, 1, cv2.NORM_MINMAX) depthDisplayImage = depthDisplayImage[:,:,0]; if centroidArray != None: msg.labelCoord = () for i in range(np.size(centroidArray,0)): msg1 = IM depthValue = getDepth(depthDisplayImage,centroidArray[i][0],centroidArray[i][1]) msg1.label = labels[i] msg1.x = centroidArray[i][0] msg1.y = centroidArray[i][1] msg1.z = depthValue msg.stamp = rospy.Time.now() msg.labelCoord = msg.labelCoord + (msg1,) msgPub.publish(msg) except KeyboardInterrupt: print "Shutting down vision node." cv.DestroyAllWindows()
# Convert the depth image to a Numpy array since most cv2 functions # require Numpy arrays. depthArray = np.array(depthImage, dtype=np.float32) # Normalize the depth image to fall between 0 (black) and 1 (white) depthDisplayImage = cv2.normalize(depthArray, depthArray, 0, 1, cv2.NORM_MINMAX) # Display the result cv2.imshow("Depth Image", depthDisplayImage) #depth_display_image = self.process_depth_image(depth_array) msg = IC() depthDisplayImage = depthDisplayImage[:,:,0]; if centroidArray != None: msg.labelCoord = () for i in range(np.size(centroidArray,0)): msg1 = IM depthValue = self.getDepth(depthDisplayImage,centroidArray[i][0],centroidArray[i][1]) msg1.label = labels[i] msg1.x = centroidArray[i][0] msg1.y = centroidArray[i][1] msg1.z = depthValue #pixelLocation3D = (labels[i],centroidArray[i][0],centroidArray[i][1],depthValue) #objectLocations3D.append(pixelLocation # msg.stamp = rospy.Time.now() msg.labelCoord = msg.labelCoord + (msg1,) #print objectLocations3D
# Convert the depth image to a Numpy array since most cv2 functions # require Numpy arrays. depthArray = np.array(depthImage, dtype=np.float32) # Normalize the depth image to fall between 0 (black) and 1 (white) depthDisplayImage = cv2.normalize(depthArray, depthArray, 0, 1, cv2.NORM_MINMAX) # Display the result cv2.imshow("Depth Image", depthDisplayImage) #depth_display_image = self.process_depth_image(depth_array) msg = IC() depthDisplayImage = depthDisplayImage[:, :, 0] if centroidArray != None: msg.labelCoord = () for i in range(np.size(centroidArray, 0)): msg1 = IM depthValue = self.getDepth(depthDisplayImage, centroidArray[i][0], centroidArray[i][1]) msg1.label = labels[i] msg1.x = centroidArray[i][0] msg1.y = centroidArray[i][1] msg1.z = depthValue #pixelLocation3D = (labels[i],centroidArray[i][0],centroidArray[i][1],depthValue) #objectLocations3D.append(pixelLocation # msg.stamp = rospy.Time.now() msg.labelCoord = msg.labelCoord + (msg1, ) #print objectLocations3D