SHARE
TWEET

Untitled

a guest Aug 20th, 2019 85 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python
  2. #Reference: the baxter_stocking_stuffer project by students in Northwestern's MSR program - Josh Marino, Sabeen Admani, Andrew Turchina and Chu-Chu Igbokwe
  3. #Service provided - ObjLocation service - contains x,y,z coordinates of object in baxter's stationary body frame, whether it is ok to grasp and if objects were found in the current frame.
  4.  
  5. import rospy
  6. import numpy as np
  7. import cv2
  8. import cv_bridge
  9. import baxter_interface
  10. import imutils
  11. import numpy
  12.  
  13. from std_msgs.msg import String, Int32
  14. from sensor_msgs.msg import Image
  15. from geometry_msgs.msg import Point
  16.  
  17. from cv_bridge import CvBridge, CvBridgeError
  18.  
  19. from baxter_builder.srv import *
  20.  
  21. '''
  22. #green
  23. g_low_h  = 60
  24. g_high_h = 90
  25. g_low_s  = 85
  26. g_high_s = 175
  27. g_low_v  = 70
  28. g_high_v = 255
  29.  
  30. #white
  31. w_low_h=0
  32. w_high_h=0
  33. w_low_s=0
  34. w_high_s=0
  35. w_low_v=0
  36. w_high_v=255
  37.  
  38. #blue
  39. b_low_h  = 105
  40. b_high_h = 115
  41. b_low_s  = 135
  42. b_high_s = 160
  43. b_low_v  = 20
  44. b_high_v = 60
  45. '''
  46.  
  47. # global obj_found
  48. obj_found = False
  49. # global correct_location
  50. correct_location = True
  51. #Object color: 0 = green, 1 = blue
  52. obj_color = 0
  53.  
  54. #Object centroid position in the baxter's stationary base frame
  55. xb = 0
  56. yb = 0
  57.  
  58.  
  59. '''
  60. Thresholds camera image and stores object centroid location (x,y) in Baxter's base frame.
  61. '''
  62. def callback(message):
  63.  
  64.     global xb, yb, obj_color, obj_found
  65.     xb = 0
  66.     yb = 0
  67.     #Capturing image of camera
  68.     bridge = CvBridge()
  69.  
  70.     cv_image = bridge.imgmsg_to_cv2(message, "bgr8")
  71.     height, width, depth = cv_image.shape
  72.     #print height, width, depth
  73.     #kernel = numpy.ones((5 ,5), numpy.uint8)
  74. #while (True):
  75.    # frame = cv_image
  76.    # rangomax = numpy.array([255, 50, 50]) # B, G, R
  77.   # rangomin = numpy.array([52, 0, 0])
  78.   #  mask = cv2.inRange(frame, rangomin, rangomax)
  79.     # reduce the noise
  80.   #  opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
  81.   #  x, y, w, h = cv2.boundingRect(opening)
  82.    # cv2.rectangle(frame, (x, y), (x+w, y + h), (0, 255, 0), 3)
  83.   #  cv2.circle(frame, (x+w/2, y+h/2), 5, (0, 0, 255), -1)
  84.   #  cv2.imshow('camera', frame)
  85.   #  k = cv2.waitKey(1) & 0xFF
  86.     hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
  87.     obj_color = 0
  88.     #cv2.namedWindow("Feed", cv2.WINDOW_NORMAL)
  89.     #Green colored objects
  90.     if obj_color == 0:
  91.         # Analyze image for green objects
  92.         low_h  = 105
  93.         high_h = 115
  94.         low_s  = 135
  95.         high_s = 160
  96.         low_v  = 20
  97.         high_v = 60
  98.     print 1
  99.    
  100.         thresholded = cv2.inRange(hsv, np.array([51,81,24]), np.array([180,255,255]))
  101.     print 22
  102.         #Morphological opening (remove small objects from the foreground)
  103.         thresholded = cv2.erode(thresholded, np.ones((2,2), np.uint8), iterations=2)
  104.         thresholded = cv2.dilate(thresholded, np.ones((2,2), np.uint8), iterations=2)
  105.     print 33
  106.          #Morphological closing (fill small holes in the foreground)
  107.         thresholded = cv2.dilate(thresholded, np.ones((2,2), np.uint8), iterations=1)
  108.         thresholded = cv2.erode(thresholded, np.ones((2,2), np.uint8), iterations=1)
  109.     print 44
  110.         ##Finding center of object
  111.         #cv2.threshold(source image in greyscale, threshold value which is used to classify the pixel values, the maxVal which represents the value to be given if pixel value is more than (sometimes less than) the threshold value )
  112.         #output: retval and thresholded image
  113.         ret,thresh = cv2.threshold(thresholded,157,255,0)
  114.     #print 55
  115.         #findCountours(source image, contour retrieval mode, contour approximation method )
  116.         #contours is a Python list of all the contours in the image. Array of arrays.
  117.         #Each individual contour is a Numpy array of (x,y) coordinates of boundary points of the object.
  118.         #hierarchy is holding information on the nesting of contours
  119.         im2, contours, hierarchy = cv2.findContours(thresholded,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE)
  120.         #can also use cv2.CHAIN_APPROX_SIMPLE that removes all redundant points and compresses the contour, thereby saving memory.
  121.     #print 66
  122.         # contour_sizes = [(cv2.contourArea(contour), contour) for contour in contours]
  123.         # biggest_contour = max(contour_sizes, key=lambda x: x[0])[1]
  124.  
  125.         # mask = np.zeros(cv_image.shape, np.uint8)
  126.         # cv2.drawContours(mask, [biggest_contour], -1, 255, -1)
  127.         #Draw the countours.
  128.         #drawCountours(source image, contours as a Python list, index of contours (useful when drawing individual contour. To draw all contours, pass -1),color, thickness)
  129.         cv2.drawContours(cv_image, contours, -1, (0,255,0), 1)
  130.     #print 77
  131.         numobj = len(contours) # number of objects found in current frame
  132.         print 'Number of objects found in the current frame: ' , numobj
  133.     #print 88
  134.         if numobj > 0:
  135.             moms = cv2.moments(contours[0])
  136.             if moms['m00']>500:
  137.                 cx = int(moms['m10']/moms['m00'])
  138.                 cy = int(moms['m01']/moms['m00'])
  139.                 print 2
  140.                 # print 'cx = ', cx
  141.                 # print 'cy = ', cy
  142.  
  143.                 #dist = baxter_interface.analog_io.AnalogIO('left_hand_range').state()
  144.                 #print "Distance is %f" % dist
  145.                 #dist = dist/1000
  146.  
  147.                 #Position in the baxter's stationary base frame
  148.                 xb = (cy - (height/2))*.0031*.461 + .712 - .02
  149.                 yb = (cx - (width/2))*.0031*.461 + .316  - .02
  150.         print xb
  151.         print yb
  152.             #print "Found green ", numobj,  "object(s)"
  153.             obj_found = True
  154.             print 99
  155.             cv2.imshow("Thresholded", thresholded)
  156.     #Printing to screen the images
  157.     cv2.imshow("Original", cv_image)
  158.     cv2.imshow("Thresholded", thresholded)
  159.     cv2.waitKey(3)
  160.  
  161. '''
  162. Creates and returns a response with object location info for the object_location_service.
  163. '''
  164. def get_obj_location(request):
  165.     global xb, yb, obj_color, correct_location, obj_found
  166.     zb = 0
  167.     while xb == 0 and yb == 0:
  168.         rospy.sleep(1)
  169.     return ObjLocationResponse(xb, yb, zb, obj_found, obj_color)
  170. '''
  171. Creates a service that provides information about the loaction of an object.
  172. Subscribes to left hand camera image feed
  173. '''
  174. def main():
  175.  
  176.     #Initiate left hand camera object detection node
  177.     rospy.init_node('left_camera_node')
  178.  
  179.     #Create names for OpenCV images and orient them appropriately
  180.     #cv2.namedWindow("Original", 1)
  181.     #cv2.namedWindow("Thresholded", 2)
  182.  
  183.     #Subscribe to left hand camera image
  184.     rospy.Subscriber("/cameras/left_hand_camera/image", Image, callback)
  185.  
  186.     #Declare object location service called object_location_srv with ObjLocation service type.
  187.     #All requests are passed to get_obj_location function
  188.     #print(get_obj_location)
  189.     obj_location_srv = rospy.Service("object_location_service", ObjLocation, get_obj_location)
  190.     #print get_obj_location
  191.    
  192.  
  193.  
  194.     print "Left - Ready to find object."
  195.  
  196.     #Keeps code from exiting until service is shutdown
  197.     rospy.spin()
  198.  
  199.  
  200. if __name__ == '__main__':
  201.      main()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top