Add comments

src/find_color.py

@@ -23,7 +23,9 @@ def nothing(x):
 class FindColor:
     def __init__(self, debug=True):
         self.bridge = CvBridge()
+        # Subscribe to the camera
         self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
+        # Create the center_deviation topic where we will publish the error
         self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
         # Dummy images
         self.cv_image = np.zeros((100, 100, 3), np.uint8)
@@ -59,9 +61,6 @@ class FindColor:
             print("Error converting the image: {}".format(e))
             return
 
-        # print("Image shape {}".format(self.cv_image.shape))
-        image_width = self.cv_image.shape[1]
-
         if self.debug:
             # Get info from track bar and appy to result
             self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
@@ -70,14 +69,16 @@ class FindColor:
             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)
+
         # Convert to HSV color space
         self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
-        # Normal masking algorithm
         mask, center, radius = None, None, 0
+
         # If valid values update the Thresholds
         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])
+            # Create the binary mask
             mask = cv2.inRange(self.hsv, lower, upper)
             # Erosion and dilation to remove noise
             kernel = np.ones((7, 7), np.uint8)
@@ -86,9 +87,10 @@ class FindColor:
             # Find contours in the mask
             contours = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
             if len(contours) > 0:
-                # Find largest contour and compute the minimum eclosing circle
+                # Find largest contour and compute the minimum enclosing circle
                 c = max(contours, key=cv2.contourArea)
                 ((x, y), radius) = cv2.minEnclosingCircle(c)
+                # Compute the centroid of the mask
                 M = cv2.moments(c)
                 center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
             else:
@@ -97,27 +99,31 @@ class FindColor:
         else:
             print("Invalid values for Thresholds")
 
+        # Create the masked image
         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 > MIN_RADIUS_SIZE and self.debug:
-            # Draw the circle
+            # Draw the enclosing circle and center on the two images
             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)
 
-        # Publish horizon deviation from the middle
+        # Publish deviation from the middle of the image (= the error)
         if center is not None:
             # print("Center found: {}".format(center))
             # center_deviation_px = int(center[0] - image_width // 2)
-            max_deviation = image_width // 2
             # Rescale the error to [-1, 1]
+            image_width = self.cv_image.shape[1]
+            max_deviation = image_width // 2
             center_deviation = (center[0] - max_deviation) / max_deviation
             # print("Center deviation : {}".format(center_deviation))
-
+            # Publish the error to the corresponding topic
             self.center_pub.publish(center_deviation)
 
         if self.debug:
+            # Show the images
             cv2.imshow(self.image_name, self.cv_image)
             cv2.imshow(self.mask_name, self.mask_image)
             # Wait 3 ms to show the images
@@ -126,8 +132,8 @@ class FindColor:
 
 if __name__ == '__main__':
 
-    ic = FindColor()
     print("Starting FindColor node")
+    ic = FindColor()
     rospy.init_node('FindColor', anonymous=True)
     try:
         rospy.spin()

src/follow_color.py

@@ -19,7 +19,7 @@ def turnCallback(data):
     # Error (deviation to the center rescaled to [-1, 1])
     error = data.data
     # print("Error: {}".format(error))
-    
+
     # Init twist message (speed + orientation command)
     vel_msg = Twist()
     vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0
@@ -33,10 +33,10 @@ def turnCallback(data):
         dt = 1
     # PID Control
     vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
+    # Update integral error
     errorI += error
-    # Clip integral error
-    # errorI = np.clip(errorI, -1, 1)
 
+    # Publish the Twist message
     vel_pub.publish(vel_msg)