diff --git a/CMakeLists.txt b/CMakeLists.txt
index e659725c0325682aab62a8515ae837bda90288ef..1ff1de85f3741f4c14899a83fc6dcff99bab2c20 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
+ geometry_msgs
message_generation
sensor_msgs
)
@@ -50,9 +51,9 @@ find_package(catkin REQUIRED COMPONENTS
## Generate messages in the 'msg' folder
add_message_files(
-# FILES
-# Message1.msg
-# Message2.msg
+ FILES
+ LabeledPoint.msg
+ PiecesCoords.msg
)
## Generate services in the 'srv' folder
@@ -71,6 +72,7 @@ add_service_files(
generate_messages(
DEPENDENCIES
std_msgs
+ geometry_msgs
)
################################################
diff --git a/scripts/contour_centroid.py b/scripts/contour_centroid.py
index d4bdd75127432b33107f8603f717d026c80a8540..e4ce7bf1b22ce66dcbcfbd3d953cf8dd30713399 100755
--- a/scripts/contour_centroid.py
+++ b/scripts/contour_centroid.py
@@ -52,10 +52,10 @@ for c in contours:
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)
- lower = np.array([1, 25, 25])
- upper = np.array([10, 255, 255])
- mask = cv2.inRange(hsv1, lower, upper)
- output = cv2.bitwise_and(img, img, mask=mask)
+# lower = np.array([1, 25, 25])
+# upper = np.array([10, 255, 255])
+# mask = cv2.inRange(hsv1, lower, upper)
+# output = cv2.bitwise_and(img, img, mask=mask)
# Threshold the HSV image to get only red colors
diff --git a/scripts/obj_recogkinect.py b/scripts/obj_recogkinect.py
index 7e1615b1e725a2292dd3f9581cfe0cdd8e306662..b0a33841da882c3a0b7aa84f1c1243f29eda77d3 100755
--- a/scripts/obj_recogkinect.py
+++ b/scripts/obj_recogkinect.py
@@ -1,44 +1,109 @@
#!/usr/bin/env python
+#Import all the required libraries
from __future__ import print_function
-# rospy for subscriber
import rospy
import cv2
-#ROS messages
-#from std_msgs.msg import String
from sensor_msgs.msg import Image
-#ROS Image message to OpenCV2 image converter
+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:
def __init__(self):
img_tpc = "camera/rgb/image_raw"
- img_tpc1 = "camera/depth/image_raw"
- self.image_pub = rospy.Publisher("img_pross2", Image, queue_size=5)
+ self.image_pub = rospy.Publisher("img_pross2", Image, self.img_callback, queue_size=10)
+ self.pub = rospy.Publisher("Coords",PiecesCoords,self.img_callback, queue_size=10)
self.image_sub = rospy.Subscriber(img_tpc, Image, self.img_callback)
- self.depth_sub = rospy.Subscriber(img_tpc1, Image, self.img_callback)
- ros_img = cv2.imread('/home/maitreyee/Pictures/IMG_20170927_090557092.jpg')
+ self.sub = rospy.Subscriber("Coords", PiecesCoords, self.img_callback, queue_size=10)
self.bridge = CvBridge()
# Convert ROS image to OpenCV2
- def img_callback(self,ros_img) :
+
+ def img_callback(self,data) :
try:
- cv2_img = self.bridge.imgmsg_to_cv2(ros_img, "bgr8")
- 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)
+ cimg = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
+ # hsv1 = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+ blur = cv2.GaussianBlur(gray, (17, 17), 0)
+ (ret, thresh) = cv2.threshold(blur, 100, 250, cv2.THRESH_BINARY)
+ ratio = img.shape[0] / float(img.shape[0])
+ self.msg = PiecesCoords()
+
+ (im2, contours, hierarchy) = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+ for c in contours:
+ shape = "unidentified"
+ peri = cv2.arcLength(c, True)
+ approx = cv2.approxPolyDP(c, 0.04 * peri, True)
+ # if the shape has 4 vertices, it is either a square or
+ # a rectangle
+ if len(approx) == 4:
+ # compute the bounding box of the contour and use the
+ # bounding box to compute the aspect ratio
+
+ (x, y, w, h) = cv2.boundingRect(approx)
+ ar = w / float(h)
+
+ 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")
+ 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"
+ self.p = Point()
+ self.p.x = cX
+ self.p.y = cY
+ self.msg.cells = [self.msg.cells, self.p]
+ return self.msg.cells
+ circles = cv2.HoughCircles(gray, cv2.HOUGH_GRADIENT, 1, 20,
+ param1=620, param2=10, minRadius=39, maxRadius=42)
+ 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)
+ self.p = Point()
+ self.lp = LabeledPoint()
+ self.p.x = i[0]
+ self.p.y = i[1]
+ self.lp.label = "O"
+ self.lp.point = self.p
+ self.msg.pieces = [self.msg.pieces, self.lp]
+ return self.msg.pieces
+
+
+ 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(cv2_img,"bgr8"))
+ self.pub.publish(self.msg.pieces)
+ except CvBridgeError as e:
+ print (e)
def main(args):
ic = image_converter()
rospy.init_node('image_converter',anonymous=True)
try:
- rospy.sleep(30)
+ rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
@@ -47,22 +112,3 @@ def main(args):
if __name__ == '__main__':
main(sys.argv)
-
-# imgproc_node_name = 'obj_recogkinect_server'
-# imgproc_topic_name = 'obj_recogkinect'
- #Topic where we provide it as service
-# self.image_service = rospy.Service(imgproc_node_name, imgproc_topic_name, self.handleimg_callback)
-# self.image_service = rospy.Service("img_pross1", Image)
- #Topic where we publish
-
- #Topic where we subscribe to Kinect
- #Define the image topic change it once you know the path
-# img_tpc = "camera/rgb/image_raw"
- #Spin until interrupted
-# img1 = cv2.imread("/home/maitreyee/pic1.png")
-# imgx = cv2.imread("/home/maitreyee/pic1.png")
-# orb = cv2.ORB_create()
-# kp = orb.detect(img1, None)
-# kp, des = orb.compute(img1, kp)
-# img2 = cv2.drawKeypoints(img1,kp,imgx,color=None, flags=2)
-# (rows,cols,channels) = img1.shape
\ No newline at end of file