diff --git a/scripts/obj_recogkinect.py b/scripts/obj_recogkinect.py new file mode 100644 index 0000000000000000000000000000000000000000..92a05c1f10d045d1dcc53f5ba266da742741ef09 --- /dev/null +++ b/scripts/obj_recogkinect.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python + +#Import all the required libraries +from __future__ import print_function +import roslib +roslib.load_manifest('your_package_name') +import rospy +import cv2 +from sensor_msgs.msg import Image +from geometry_msgs.msg import Point +from cv_bridge import CvBridge, CvBridgeError +import sys +import numpy as np +from socrates_workshop.msg import PiecesCoords, LabeledPoint + + +def filter_cell(c): + perimeter = cv2.arcLength(c, True) + area = cv2.contourArea(c) + approx = cv2.approxPolyDP(c, 0.04 * perimeter, True) + if len(approx) == 4 and area < 10000: + return True + else: + return False + + +def filter_circle(contour): + perimeter = cv2.arcLength(contour, True) + approx = cv2.approxPolyDP(contour, 0.01 * perimeter, True) + area = cv2.contourArea(contour) + if ((len(approx) > 8) & (area < 50)): + return True + else: + return False + + +def filter_cross(contour): + # returns true if contour is a square, false otherwise + pass + +def filter_contours(contours, filter_function): + result = [] + leftover = [] + for c in contours: + if filter_function(c): + result = result + [c] + else: + leftover = leftover + [c] + + return [result, leftover] + +def find_center_point(c, ratio): + p = Point() + M = cv2.moments(c) + cX = int((M["m10"] / M["m00"]) * ratio) + cY = int((M["m01"] / M["m00"]) * ratio) + p.x = cX + p.y = cY + return p + +#class for image conversion +class image_converter: + +#initialize subscribe, publish, bridge and msg services for call back + def __init__(self): + img_tpc = "/rgb/image_raw" + self.image_pub = rospy.Publisher("img_pross2", Image, queue_size=10) + self.pub = rospy.Publisher("Coords",PiecesCoords, queue_size=10) + self.image_sub = rospy.Subscriber(img_tpc, Image, self.img_callback) + self.bridge = CvBridge() + + + # Convert ROS image to OpenCV2, do the image processing and publish the image and array of coordinates + + def img_callback(self, data): + try: + cv2_img = self.bridge.imgmsg_to_cv2(data, "bgr8") + #rezie the image to fit the output window. + img = cv2.resize(cv2_img, (640, 480), interpolation=cv2.INTER_CUBIC) + #convert the image to gray scale for blurring them and later used for finding contours. + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + #If objects use hsv then convert the gray image to rgb for further processing + cimg = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR) + #then convert the rgb to hsv + # # hsv1 = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + #Blurring the image to process it further + blur = cv2.GaussianBlur(gray, (17, 17), 0) + #find out the treshold values by tweking them + (ret, thresh) = cv2.threshold(blur, 100, 250, cv2.THRESH_BINARY) + #Ratio of shape of the image + ratio = img.shape[0] / float(img.shape[0]) + # Find out all the internal contours for sqaures + (im2, contours, hierarchy) = cv2.findContours(thresh, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE) + + [cells, contours] = filter_contours(contours, filter_cell) + [circles, contours] = filter_contours(contours, filter_circle) + [crosses, contours] = filter_contours(contours, filter_cross) + + self.msg = PiecesCoords() + + self.msg.cells = [find_center_point(c, ratio) for c in cells] + self.msg.pieces = self.msg.pieces + [LabeledPoint(find_center_point(c, ratio), "O") for c in circles] + self.msg.pieces = self.msg.pieces + [LabeledPoint(find_center_point(c, ratio), "X") for c in crosses] + + + """ + #Loop through each contour to find out the shape and centre of each cell + for c in contours: + shape = "unidentified" + perimeter = cv2.arcLength(c, True) + area = cv2.contourArea(c) + approx = cv2.approxPolyDP(c, 0.04 * perimeter, True) + # if the shape has 4 vertices, it is either a square or + # a rectangle and filter the sqaures based on area. + #Here we aim for only the smaller squares or rectangles which represent the grid + if len(approx) == 4 and area < 10000: + # compute the bounding box of the contour and use the + # bounding box to compute the aspect ratio + + (x, y, w, h) = cv2.boundingRect(approx) + #Compute the aspect ratio to differentiate between + #different shapes. + ar = w / float(h) + #here we find out the coordinates for each cell of the grid. + M = cv2.moments(c) + cX = int((M["m10"] / M["m00"]) * ratio) + cY = int((M["m01"] / M["m00"]) * ratio) + c = c.astype("float") + c *= ratio + c = c.astype("int") + #Draw the contours and their centroid + cv2.drawContours(img, [c], -1, (0, 255, 0), 2) + cv2.circle(img, (cX, cY), 7, (255, 255, 255), -1) + + # a square will have an aspect ratio that is approximately + # equal to one, otherwise, the shape is a rectangle + shape = "square" if ar >= 0.95 and ar <= 1.05 else "rectangle" + #Convert then to ROS messages + p = Point() + p.x = cX + p.y = cY + self.msg.cells = self.msg.cells + [p] + + + #Here we apply Hough transformation to find out the centroid and bounding of a cricle in the grid + circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 20, + param1=200, param2=10, minRadius=30, maxRadius=50) + if circles is not None: + circles1 = np.uint16(np.around(circles)) + for i in circles1[0, :]: + # draw the center of the circles + cv2.circle(cimg, (i[0], i[1]), i[2], (0, 255, 0), 2) + cv2.circle(cimg, (i[0], i[1]), 2, (0, 0, 255), 3) + p = Point() + lp = LabeledPoint() + p.x = i[0] + p.y = i[1] + lp.label = "O" + lp.point = p + self.msg.pieces = self.msg.pieces + [lp] + + if crosses is not None: + crosses1 = np.uint16(np.around(crosses)) + for i in crosses1[0, :]: + # draw the center of the circles + cv2.circle(cimg, (i[0], i[1]), i[2], (0, 255, 0), 2) + cv2.circle(cimg, (i[0], i[1]), 2, (0, 0, 255), 3) + p = Point() + lp = LabeledPoint() + p.x = i[0] + p.y = i[1] + lp.label = "O" + lp.point = p + self.msg.pieces = self.msg.pieces + [lp] + + + except CvBridgeError as e: + print(e) + cv2.imshow("Image window",cv2_img) + cv2.waitKey(0) + + """ + + try: + self.image_pub.publish(self.bridge.cv2_to_imgmsg(img,"bgr8")) + self.pub.publish(self.msg) + except CvBridgeError as e: + print (e) + + +def main(args): + rospy.init_node('image_converter',anonymous=True) + ic = image_converter() + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + + +if __name__ == '__main__': + main(sys.argv) +