修改

src/rasp_camera/scripts/color_recognition2.py

@@ -26,7 +26,7 @@ range_rgb = {"red": (0, 0, 255),
               "green": (0, 255, 0),
               "black": (0, 0, 0),
               }
-
+kernel = np.ones((5, 5), np.uint8)
 target_color = "green"
 class Color_Recognition:
     
@@ -53,36 +53,30 @@ class Color_Recognition:
     def image_callback(self, msg):
         image = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="bgr8")
 
-        img_center_x = image.shape[:2][1]/2
-        img_center_y = image.shape[:2][0]/2
-
+        # img_center_x = image.shape[:2][1]/2
+        # img_center_y = image.shape[:2][0]/2
+
+        # img_gaussian_blur = cv2.GaussianBlur(image, (5, 5), 0)
+        # img_hsv = cv2.cvtColor(img_gaussian_blur, cv2.COLOR_BGR2HSV)
+        # img_erode = cv2.erode(img_hsv, None, iterations=2)
+        # img_mask = cv2.inRange(img_erode, color_range[target_color][0], color_range[target_color][1])
+        # contours = cv2.findContours(img_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
+        # area_max_contour = self.get_area_max_contour(contours)[0]
+        # if area_max_contour is not None:    
+        #     rect = cv2.minAreaRect(area_max_contour)
+        #     box = cv2.boxPoints(rect)
+        #     cv2.drawContours(image, [np.int0(box)], -1, (0, 255, 255), 2)
         img_gaussian_blur = cv2.GaussianBlur(image, (5, 5), 0)
         img_hsv = cv2.cvtColor(img_gaussian_blur, cv2.COLOR_BGR2HSV)
         img_erode = cv2.erode(img_hsv, None, iterations=2)
         img_mask = cv2.inRange(img_erode, color_range[target_color][0], color_range[target_color][1])
-        contours = cv2.findContours(img_mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
-        # if len(contours) == 0:
-        #     return
-        # area_max_contour = max(contours, key=cv2.contourArea)
-
-        # img_lab = cv2.cvtColor(image, cv2.COLOR_BGR2LAB)
-        # img_mask = cv2.inRange(img_lab, color_range[target_color][0], color_range[target_color][1])
-
-        # opened = cv2.morphologyEx(img_mask, cv2.MORPH_OPEN, np.ones((3,3),np.uint8))
-        # closed = cv2.morphologyEx(opened, cv2.MORPH_CLOSE, np.ones((3,3),np.uint8))
-        # contours = cv2.findContours(closed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
-        # contours = cv2.findContours(img_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
-        area_max_contour = self.get_area_max_contour(contours)[0]
-
-        center_x = 0
-        center_y = 0
-        radius = 0
+        img_opening = cv2.morphologyEx(img_mask, cv2.MORPH_OPEN, kernel)
+        img_edges = cv2.Canny(img_opening, 50, 100)
+        circles = cv2.HoughCircles(img_edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=10,
+            minRadius=10, maxRadius=500)
+        # if circles is not None:
 
-        if area_max_contour is not None:
-            (center_x, center_y), radius = cv2.minEnclosingCircle(area_max_contour)
-            if radius >= 8:
-                cv2.circle(image, (int(center_y), int(center_y)), int(radius), range_rgb[target_color], 2)
-        cv2.imshow("circle", image)
+        cv2.imshow("circle", img_edges)
         cv2.waitKey(3)
 
 if __name__ == "__main__":