Fourth commit

launch/tic_tac_toe.launch

@@ -25,7 +25,7 @@
 
 	<!-- MAITREYEE PARAMETERS -->
 
-	<param name="topic_coords_pieces" 			value="coords_pieces" 			type="string" />
+	<param name="topic_coords_pieces" 			value="coords" 			type="string" />
 
 	<param name="param_human_player_label" 		value="O" 					type="string" />
 	<param name="param_ai_player_label" 		value="X" 					type="string" />
@@ -57,6 +57,7 @@
 	<node name="node_human_helper" 			pkg="socrates_workshop" 			type="node_human_helper.py" />
 	<node name="node_coords_to_board" 			pkg="socrates_workshop" 			type="node_coords_to_board.py" />
 	<node name="node_soundplay" 				pkg="sound_play" 				type="soundplay_node.py" />
+	<node name="node_coords" 				pkg="socrates_workshop" 			type="obj_recogkinect.py" />
 </launch>
 
 

scripts/node_coords_to_board.py

@@ -8,7 +8,7 @@ from socrates_workshop.msg import LabeledPoint
 from socrates_workshop.msg import PiecesCoords
 from geometry_msgs.msg import Point
 
-from socrates_workshop.srv import Board
+from socrates_workshop.srv import BoardResponse, Board
 
 NODE_NAME 		= "node_coords_to_board"
 
@@ -35,9 +35,8 @@ def closest_point(node, nodes):
 """
 Returns the cell number (in 0-8) given its point
 """
-def getCellNo(cell_point):
-	"TODO"
-	return 0
+def getCellNo(cell_point, cells, i):
+	return i
 
 def getPlayerId(label):
 	u_l = ttt.getUserPieceLabel()
@@ -65,11 +64,13 @@ def handleCoordsMessage(msg):
 
 	cells_points = [[cells[i].x, cells[i].y] for i in range(n_cells)]
 	
-	pieces_dict = [[getCellNo(closest_point(pieces_points[i], cells_points)), getPlayerId(pieces_labels[i])] for i in range(n_pieces)] 
+	pieces_dict = [[getCellNo(closest_point(pieces_points[i], cells_points), cells_points, i), getPlayerId(pieces_labels[i])] for i in range(n_pieces)] 
 
 	
 	this.current_board = [0, 0, 0, 0, 0, 0, 0, 0, 0]
 
+	pieces_dict = pieces_dict[:8]
+
 	for i in range(len(pieces_dict)):
 		cell_no = pieces_dict[i][0]
 		player_id = pieces_dict[i][1] 

scripts/node_img_rec.py

+#!/usr/bin/env python
+
+#Import all the required libraries
+from __future__ import print_function
+import roslib
+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
+
+#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()
+
+        self.img_w = 640
+        self.img_h = 480
+        k = 5
+
+        template = cv2.imread('/home/michele/test1.jpg')
+        template = cv2.cvtColor(template, cv2.COLOR_BGR2GRAY)
+        self.template = cv2.resize(template, (int(self.img_w / k), int(self.img_h / k)), interpolation=cv2.INTER_CUBIC)
+
+
+
+        # Convert ROS image to OpenCV2, do the image processing and publish the image and array of coordinates
+
+    def img_callback(self, data):
+            cv2_img = self.bridge.imgmsg_to_cv2(data, "bgr8")
+
+            img = cv2.resize(cv2_img, (self.img_w, self.img_h), interpolation=cv2.INTER_CUBIC)
+
+            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+
+            res = cv2.matchTemplate(gray,self.template,cv2.TM_CCOEFF_NORMED)
+            threshold = 0.5
+            loc = np.where( res >= threshold)
+            w, h = self.template.shape[::-1]
+
+            ii = 0
+            for pt in zip(*loc[::-1]):
+                cv2.rectangle(img, pt, (pt[0] + w, pt[1] + h), (0,ii,ii), 2)
+                ii = ii+1
+
+
+            print(ii)
+            self.image_pub.publish(self.bridge.cv2_to_imgmsg(img,"bgr8"))
+            #self.pub.publish(self.msg)
+
+
+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)

scripts/obj_recogkinect.py

@@ -3,7 +3,6 @@
 #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
@@ -14,6 +13,7 @@ import numpy as np
 from socrates_workshop.msg import PiecesCoords, LabeledPoint
 
 
+
 def filter_cell(c):
     perimeter = cv2.arcLength(c, True)
     area = cv2.contourArea(c)
@@ -26,18 +26,24 @@ def filter_cell(c):
 
 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)):
+    approx = cv2.approxPolyDP(contour, 0.04 * perimeter, True)
+    if len(approx) > 2 and len(approx) <= 8 and area < 5000:
         return True
     else:
         return False
 
 
 def filter_cross(contour):
-    # returns true if contour is a square, false otherwise
-    pass
-
+    perimeter = cv2.arcLength(contour, True)
+    area = cv2.contourArea(contour)
+    approx = cv2.approxPolyDP(contour, 0.03 * perimeter, True)
+    if len(approx) > 6 and area > 2000:
+        return True
+    else:
+        return False
+    
+	
 def filter_contours(contours, filter_function):
     result = []
     leftover = []
@@ -52,150 +58,85 @@ def filter_contours(contours, filter_function):
 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)
+    cX = int((M["m10"] / max(M["m00"], 0.001)) * ratio)
+    cY = int((M["m01"] / max(M["m00"], 0.001)) * ratio)
     p.x = cX
     p.y = cY
     return p
 
-#class for image conversion
+
+
+def print_contour_stats(img, c, color):
+    ratio = img.shape[0] / float(img.shape[0])
+    c = c.astype("float")
+    c *= ratio
+    c = c.astype("int")
+    cv2.drawContours(img, [c], -1, color, 2)
+    M = cv2.moments(c)
+    cX = int((M["m10"] / max(M["m00"], 0.001)) * ratio)
+    cY = int((M["m01"] / max(M["m00"], 0.001)) * ratio)
+    cv2.putText(img,str(cv2.contourArea(c)),(cX-20,cY-20),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,2)
+
+
+    perimeter = cv2.arcLength(c, True)
+    approx = cv2.approxPolyDP(c, 0.03 * perimeter, True)
+
+    cv2.putText(img, str(len(approx)),(cX-20,cY-10),cv2.FONT_HERSHEY_SIMPLEX, 0.5, color,2)
+
+	
+
+
 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_pub = rospy.Publisher("img_proc/contours_stats", Image, queue_size=10)
+        self.pub = rospy.Publisher("/ttt/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)
+        cv2_img = self.bridge.imgmsg_to_cv2(data, "bgr8")
+
+        img = cv2.resize(cv2_img, (640, 480), interpolation=cv2.INTER_CUBIC)
+
+        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
+
+        blur = cv2.GaussianBlur(gray, (5, 5), 0)
+
+        (ret, thresh) = cv2.threshold(blur, 100, 250, cv2.THRESH_BINARY)
+
+        ratio = img.shape[0] / float(img.shape[0])
+
+        (im2, contours, hierarchy) = cv2.findContours(thresh, cv2.RETR_CCOMP, cv2.CHAIN_APPROX_SIMPLE)
+
+        [cells, c1] = filter_contours(contours, filter_cell)
+        [circles, c2] = filter_contours(c1, filter_circle)
+        [crosses, c3] = filter_contours(c2, 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]
+
+
+        self.msg.cells = sorted(self.msg.cells, key=lambda p: p.x + 100*p.y)
+
+        [print_contour_stats(img, c, (255,0,0)) for c in cells]
+        [print_contour_stats(img, c, (0,255,0)) for c in circles]
+        [print_contour_stats(img, c, (0,0,255)) for c in crosses]
+
+
+        self.image_pub.publish(self.bridge.cv2_to_imgmsg(img,"bgr8"))
+        self.pub.publish(self.msg)
 
 
 def main(args):
     rospy.init_node('image_converter',anonymous=True)
     ic = image_converter()
-    try:
-        rospy.spin()
-    except KeyboardInterrupt:
-       print("Shutting down")
-    cv2.destroyAllWindows()
+    rospy.spin()
 
 
 if __name__ == '__main__':