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()
except CvBridgeError, e:
print e
# 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,)
except CvBridgeError, e:
print e
# 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()
