find_color.py 6.18 KB
Newer Older
Antonin Raffin's avatar
Antonin Raffin committed
1
2
3
4
5
6
7
8
#!/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
Antonin Raffin's avatar
Antonin Raffin committed
9
from std_msgs.msg import Float32
Antonin Raffin's avatar
Antonin Raffin committed
10

Antonin Raffin's avatar
Antonin Raffin committed
11
12
13
14
15
16
17
18
19
20
21
H_MIN, S_MIN, V_MIN = 0, 0, 0
H_MAX, S_MAX, V_MAX = 179, 255, 255
MIN_RADIUS_SIZE = 10

# Green Thresholds
GREEN_LOWER = [29, 86, 6]
GREEN_UPPER = [84, 255, 255]

# Dummy callback for trackbar initialization
def nothing(x):
    pass
Antonin Raffin's avatar
Antonin Raffin committed
22
23

class FindColor:
24
    def __init__(self, debug=False):
Antonin Raffin's avatar
Antonin Raffin committed
25
        self.bridge = CvBridge()
Antonin Raffin's avatar
Antonin Raffin committed
26
        # Subscribe to the camera
Antonin Raffin's avatar
Antonin Raffin committed
27
        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
28

Antonin Raffin's avatar
Antonin Raffin committed
29
        # Create the center_deviation topic where we will publish the error
Antonin Raffin's avatar
Antonin Raffin committed
30
        self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
31
32
        # Create the ball_radius topic where we will publish the radius of the ball
        self.ball_radius_pub = rospy.Publisher('ball_radius', Float32, queue_size = 10)
Antonin Raffin's avatar
Antonin Raffin committed
33
        # Dummy images
Antonin Raffin's avatar
Antonin Raffin committed
34
35
        self.cv_image = np.zeros((100, 100, 3), np.uint8)
        self.hsv = np.zeros((100, 100, 3), np.uint8)
Antonin Raffin's avatar
Antonin Raffin committed
36
37
38
39
40
41
42
43
44
        # Init Thresholds
        self.h_min, self.s_min, self.v_min = GREEN_LOWER
        self.h_max, self.s_max, self.v_max = GREEN_UPPER
        self.debug = debug
        if debug:
            self.image_name, self.mask_name = "RGB", "Mask"
            # Create Windows
            cv2.namedWindow(self.image_name)
            cv2.namedWindow(self.mask_name)
45
            cv2.waitKey(1000)
Antonin Raffin's avatar
Antonin Raffin committed
46
47
48
49
50
51
52
53
54
55
56
57
58
            # Creating track bar
            for min_max in ['min', 'max']:
                cv2.createTrackbar('h_{}'.format(min_max), self.mask_name, H_MIN, H_MAX, nothing)
                cv2.createTrackbar('s_{}'.format(min_max), self.mask_name, S_MIN, S_MAX, nothing)
                cv2.createTrackbar('v_{}'.format(min_max), self.mask_name, V_MIN, V_MAX, nothing)

            # Initial value, for green
            cv2.setTrackbarPos('h_min', self.mask_name, self.h_min)
            cv2.setTrackbarPos('s_min', self.mask_name, self.s_min)
            cv2.setTrackbarPos('v_min', self.mask_name, self.v_min)
            cv2.setTrackbarPos('h_max', self.mask_name, self.h_max)
            cv2.setTrackbarPos('s_max', self.mask_name, self.s_max)
            cv2.setTrackbarPos('v_max', self.mask_name, self.v_max)
Antonin Raffin's avatar
Antonin Raffin committed
59
60
61
62
63

    def callback(self, data):
        try:
            self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
        except CvBridgeError as e:
Antonin Raffin's avatar
Antonin Raffin committed
64
65
66
67
68
69
70
71
72
73
            print("Error converting the image: {}".format(e))
            return
        if self.debug:
            # Get info from track bar and appy to result
            self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
            self.s_min = cv2.getTrackbarPos('s_min', self.mask_name)
            self.v_min = cv2.getTrackbarPos('v_min', self.mask_name)
            self.h_max = cv2.getTrackbarPos('h_max', self.mask_name)
            self.s_max = cv2.getTrackbarPos('s_max', self.mask_name)
            self.v_max = cv2.getTrackbarPos('v_max', self.mask_name)
Antonin Raffin's avatar
Antonin Raffin committed
74

Antonin Raffin's avatar
Antonin Raffin committed
75
76
        # Convert to HSV color space
        self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
Antonin Raffin's avatar
Antonin Raffin committed
77
        mask, center, radius = None, None, 0
Antonin Raffin's avatar
Antonin Raffin committed
78

Antonin Raffin's avatar
Antonin Raffin committed
79
        # If valid values update the Thresholds
Antonin Raffin's avatar
Antonin Raffin committed
80
81
82
        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])
Antonin Raffin's avatar
Antonin Raffin committed
83
            # Create the binary mask
Antonin Raffin's avatar
Antonin Raffin committed
84
            mask = cv2.inRange(self.hsv, lower, upper)
Antonin Raffin's avatar
Antonin Raffin committed
85
86
87
88
89
90
91
            # 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
            contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
            if len(contours) > 0:
Antonin Raffin's avatar
Antonin Raffin committed
92
                # Find largest contour and compute the minimum enclosing circle
Antonin Raffin's avatar
Antonin Raffin committed
93
94
                c = max(contours, key=cv2.contourArea)
                ((x, y), radius) = cv2.minEnclosingCircle(c)
Antonin Raffin's avatar
Antonin Raffin committed
95
                # Compute the centroid of the mask
Antonin Raffin's avatar
Antonin Raffin committed
96
97
98
99
100
101
102
                M = cv2.moments(c)
                center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
            else:
                pass
                # print("No center found")
        else:
            print("Invalid values for Thresholds")
Antonin Raffin's avatar
Antonin Raffin committed
103

Antonin Raffin's avatar
Antonin Raffin committed
104
        # Create the masked image
Antonin Raffin's avatar
Antonin Raffin committed
105
        self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
106

Antonin Raffin's avatar
Antonin Raffin committed
107
        # Only proceed if the radius meets a minimum size
Antonin Raffin's avatar
Antonin Raffin committed
108
        if radius > MIN_RADIUS_SIZE and self.debug:
Antonin Raffin's avatar
Antonin Raffin committed
109
            # Draw the enclosing circle and center on the two images
Antonin Raffin's avatar
Antonin Raffin committed
110
111
112
113
            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)
Antonin Raffin's avatar
Antonin Raffin committed
114

Antonin Raffin's avatar
Antonin Raffin committed
115
        # Publish deviation from the middle of the image (= the error)
Antonin Raffin's avatar
Antonin Raffin committed
116
        if center is not None:
Antonin Raffin's avatar
Antonin Raffin committed
117
118
119
            # print("Center found: {}".format(center))
            # center_deviation_px = int(center[0] - image_width // 2)
            # Rescale the error to [-1, 1]
Antonin Raffin's avatar
Antonin Raffin committed
120
121
            image_width = self.cv_image.shape[1]
            max_deviation = image_width // 2
Antonin Raffin's avatar
Antonin Raffin committed
122
123
            center_deviation = (center[0] - max_deviation) / max_deviation
            # print("Center deviation : {}".format(center_deviation))
Antonin Raffin's avatar
Antonin Raffin committed
124
            # Publish the error to the corresponding topic
Antonin Raffin's avatar
Antonin Raffin committed
125
            self.center_pub.publish(center_deviation)
Antonin Raffin's avatar
Antonin Raffin committed
126

127
128
129
130
        # Publish the ball_radius
        if radius > MIN_RADIUS_SIZE:
            self.ball_radius_pub.publish(radius)

Antonin Raffin's avatar
Antonin Raffin committed
131
        if self.debug:
Antonin Raffin's avatar
Antonin Raffin committed
132
            # Show the images
Antonin Raffin's avatar
Antonin Raffin committed
133
            cv2.imshow(self.image_name, self.cv_image)
134
            cv2.imshow(self.masked, self.mask_image)
Antonin Raffin's avatar
Antonin Raffin committed
135
136
            # Wait 3 ms to show the images
            cv2.waitKey(3)
Antonin Raffin's avatar
Antonin Raffin committed
137
138
139
140


if __name__ == '__main__':

Antonin Raffin's avatar
Antonin Raffin committed
141
    print("Starting FindColor node")
Antonin Raffin's avatar
Antonin Raffin committed
142
    rospy.init_node('FindColor', anonymous=True)
143
    ic = FindColor()
Antonin Raffin's avatar
Antonin Raffin committed
144
145
146
147
148
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down")
        cv2.destroyAllWindows()