diff --git a/shape_recognizer.py b/shape_recognizer.py index e336b2f..9fa4653 100755 --- a/shape_recognizer.py +++ b/shape_recognizer.py @@ -25,9 +25,10 @@ def __init__(self): self.minVal = 50 self.maxVal = 87 self.test_image = cv2.imread("./circle_base.png",-1) - if self.test_image: - self.test_image = cv2.medianBlur(img,5) - self.edge_detected = cv2.Canny(self.test_image,self.minVal,self.maxVal) + # if self.test_image: + self.test_image = cv2.medianBlur(self.test_image,5) + self.edge_detected = cv2.Canny(self.test_image,self.minVal,self.maxVal) + @@ -64,7 +65,7 @@ def process_image(self, msg): """ Process image messages from ROS and stash them in an attribute called cv_image for subsequent processing """ self.cv_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") - self.cv_image = cv2.medianBlur(img,5) + self.cv_image = cv2.medianBlur(self.cv_image,5) self.gray_image = cv2.adaptiveThreshold(self.cv_image,255,cv2.ADAPTIVE_THRESH_GAUSSIAN_C,\ cv2.THRESH_BINARY,11,2) # self.gray_image = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY) @@ -76,6 +77,9 @@ def process_image(self, msg): # (158,255,236)) self.edge_detected = cv2.Canny(self.cv_image,self.minVal,self.maxVal) + + + _, self.contours, self.contour_hierarchy = cv2.findContours(self.edge_detected, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) self.contour_image = cv2.drawContours(self.cv_image, self.contours, -1, (0,255,0), 3) print dp(self.edge_detected, 1) @@ -154,6 +158,8 @@ def set_maxVal(self, val): def run(self): """ The main run loop""" r = rospy.Rate(10) + + print dp(self.edge_detected, 1) while not rospy.is_shutdown(): if not self.cv_image is None: