重写颜色识别代码

src/rasp_camera/scripts/color_recognition2.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+import rospy, cv2, cv_bridge, math, datetime
+import numpy as np
+from sensor_msgs.msg import CompressedImage
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64
+
+# color_range = {
+# "red": [(0, 164, 66), (255, 255, 255)], 
+# "green": [(19, 0, 0), (255, 108, 255)], 
+# "blue": [(17, 0, 0), (250, 255, 110)], 
+# "black": [(0, 0, 0), (56, 255, 255)], 
+# "white": [(193, 0, 0), (255, 250, 255)], 
+#               }
+
+color_range = {
+"red": [(0, 43, 46), (10, 255, 255)], 
+"green": [(35, 43, 46), (77, 255, 255)], 
+"blue": [(100, 43, 46), (124, 255, 255)], 
+"black": [(0, 0, 0), (56, 255, 255)], 
+"white": [(193, 0, 0), (255, 250, 255)], 
+              }
+
+range_rgb = {"red": (0, 0, 255),
+              "blue": (255, 0,0),
+              "green": (0, 255, 0),
+              "black": (0, 0, 0),
+              }
+
+target_color = "green"
+class Color_Recognition:
+    
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        self.image_sub = rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage, self.image_callback)
+        self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
+        self.twist = Twist()
+
+    def get_area_max_contour(self,contours) :
+        contour_area_temp = 0
+        contour_area_max = 0
+        area_max_contour = None
+
+        for c in contours : 
+            contour_area_temp = math.fabs(cv2.contourArea(c)) 
+            if contour_area_temp > contour_area_max :
+                contour_area_max = contour_area_temp
+                if contour_area_temp > 64:  
+                    area_max_contour = c
+
+        return area_max_contour, contour_area_max
+
+    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_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
+
+        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.waitKey(3)
+
+if __name__ == "__main__":
+    rospy.init_node('color_recognition')
+    Color_Recognition()
+    rospy.spin()
+
+