find_color.py 4.27 KB
Newer Older
Antonin Raffin's avatar
Antonin Raffin committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
#!/usr/bin/env python
from __future__ import print_function, division

import cv2
import numpy as np
import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Int32


class FindColor:
    def __init__(self):
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
        self.center_pub = rospy.Publisher('center_deviation', Int32, queue_size=10)
        self.cv_image = np.zeros((100, 100, 3), np.uint8)
        self.hsv = np.zeros((100, 100, 3), np.uint8)
        self.h_min = 0
        self.s_min = 0
        self.v_min = 0

        self.h_max = 179
        self.s_max = 255
        self.v_max = 255
        cv2.namedWindow('RGB')
        cv2.namedWindow('Mask')
        # Creating track bar
        cv2.createTrackbar('h_min', 'Mask', 0, 179, self.nothing)
        cv2.createTrackbar('s_min', 'Mask', 0, 255, self.nothing)
        cv2.createTrackbar('v_min', 'Mask', 0, 255, self.nothing)
        cv2.createTrackbar('h_max', 'Mask', 0, 179, self.nothing)
        cv2.createTrackbar('s_max', 'Mask', 0, 255, self.nothing)
        cv2.createTrackbar('v_max', 'Mask', 0, 255, self.nothing)
        # Initial value, for green
        cv2.setTrackbarPos('h_min', 'Mask', 29)
        cv2.setTrackbarPos('s_min', 'Mask', 86)
        cv2.setTrackbarPos('v_min', 'Mask', 6)
        cv2.setTrackbarPos('h_max', 'Mask', 84)
        cv2.setTrackbarPos('s_max', 'Mask', 255)
        cv2.setTrackbarPos('v_max', 'Mask', 255)

    def nothing(self, x):
        pass

    def callback(self, data):
        try:
            self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
            print(e)

        # get info from track bar and appy to result
        self.h_min = cv2.getTrackbarPos('h_min', 'Mask')
        self.s_min = cv2.getTrackbarPos('s_min', 'Mask')
        self.v_min = cv2.getTrackbarPos('v_min', 'Mask')
        self.h_max = cv2.getTrackbarPos('h_max', 'Mask')
        self.s_max = cv2.getTrackbarPos('s_max', 'Mask')
        self.v_max = cv2.getTrackbarPos('v_max', 'Mask')
        # Convert to HSV color space
        self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
        # Normal masking algorithm
        mask = None
        if self.h_min <= self.h_max and self.s_min <= self.s_max and self.v_min <= self.v_max:
            lower = np.array([self.h_min, self.s_min, self.v_min])
            upper = np.array([self.h_max, self.s_max, self.v_max])
            mask = cv2.inRange(self.hsv, lower, upper)
        # Erosion and dilation to remove noise
        kernel = np.ones((7, 7), np.uint8)
        mask = cv2.erode(mask, kernel, iterations=2)
        mask = cv2.dilate(mask, kernel, iterations=2)
        # Find contours in the mask
        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
        center = None
        radius = 0
        if len(cnts) > 0:
            # Find largest contour and compute the minimum eclosing circle
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

        self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
        # Only proceed if the radius meets a minimum size
        if radius > 10:
            # Draw the circle
            cv2.circle(self.mask_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
            cv2.circle(self.mask_image, center, 5, (0, 0, 255), -1)
            cv2.circle(self.cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
            cv2.circle(self.cv_image, center, 5, (0, 0, 255), -1)
        cv2.imshow("RGB", self.cv_image)
        cv2.imshow("Mask", self.mask_image)
        print(center)
        # Publish horizon deviation from the middle
        if center is not None:
            center_deviation = int(center[0] - 320)
            print(center_deviation)
            self.center_pub.publish(center_deviation)
        cv2.waitKey(3)


if __name__ == '__main__':

    ic = FindColor()
    print("start")
    rospy.init_node('FindColor', anonymous=True)
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()