Add follower in distance based on the radius of the green ball

src/find_color.py

@@ -21,12 +21,15 @@ def nothing(x):
     pass
 
 class FindColor:
-    def __init__(self, debug=True):
+    def __init__(self, debug=False):
         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)
+        # 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)
         # Dummy images
         self.cv_image = np.zeros((100, 100, 3), np.uint8)
         self.hsv = np.zeros((100, 100, 3), np.uint8)
@@ -34,12 +37,12 @@ class FindColor:
         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)
+            cv2.waitKey(1000)
             # Creating track bar
             for min_max in ['min', 'max']:
                 cv2.createTrackbar('h_{}'.format(min_max), self.mask_name, H_MIN, H_MAX, nothing)
@@ -60,7 +63,6 @@ class FindColor:
         except CvBridgeError as e:
             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)
@@ -101,7 +103,7 @@ class FindColor:
 
         # 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 enclosing circle and center on the two images
@@ -122,10 +124,14 @@ class FindColor:
             # Publish the error to the corresponding topic
             self.center_pub.publish(center_deviation)
 
+        # Publish the ball_radius
+        if radius > MIN_RADIUS_SIZE:
+            self.ball_radius_pub.publish(radius)
+
         if self.debug:
             # Show the images
             cv2.imshow(self.image_name, self.cv_image)
-            cv2.imshow(self.mask_name, self.mask_image)
+            cv2.imshow(self.masked, self.mask_image)
             # Wait 3 ms to show the images
             cv2.waitKey(3)
 
@@ -133,8 +139,8 @@ class FindColor:
 if __name__ == '__main__':
 
     print("Starting FindColor node")
-    ic = FindColor()
     rospy.init_node('FindColor', anonymous=True)
+    ic = FindColor()
     try:
         rospy.spin()
     except KeyboardInterrupt:

src/follow_color.py

@@ -2,28 +2,31 @@
 from __future__ import print_function, division
 
 import time
-
+import message_filters
 import rospy
 import numpy as np
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Float32
 
 vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
-Kp = 2
+Kp_w = 2
 Kd = 1
 Ki = 0
 last_time = None
 
+Kp_x = 0.2
+
+vel_x = 0
+omega_z = 0
+
 def turnCallback(data):
-    global errorD, errorI, last_time
+
+    global errorD, errorI, last_time, omega_z
     # 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
-    vel_msg.angular.x, vel_msg.angular.y = 0, 0
+
     if last_time is not None:
         errorD = error - last_error
         dt = time.time() - last_time
@@ -32,18 +35,43 @@ def turnCallback(data):
         errorI, errorD = 0, 0
         dt = 1
     # PID Control
-    vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
+    omega_z = - (Kp_w * error + Kd * (errorD / dt) + Ki * dt * errorI)
     # Update integral error
     errorI += error
 
+    # Init twist message (speed + orientation command)
+    vel_msg = Twist()
+    vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
+    vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z = 0, 0, omega_z
     # Publish the Twist message
     vel_pub.publish(vel_msg)
 
+def translateCallback(raw_data):
+    data=np.mean(raw_data.data)
+
+    global vel_x
+    # Error (deviation to the center rescaled to [-1, 1])
+    error = (data - 45)/20
+    # print("Error: {}".format(error))
+
+    # PID Control
+    vel_x = - Kp_x * error
+
+
+    # Init twist message (speed + orientation command)
+    vel_msg = Twist()
+    vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
+    vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z= 0, 0, omega_z
+    # Publish the Twist message
+    vel_pub.publish(vel_msg)
 
 if __name__ == "__main__":
     print("Starting follow_color node")
     rospy.init_node('follow_color', anonymous=True)
     turn_sub = rospy.Subscriber("center_deviation", Float32, turnCallback)
+    translate_sub = message_filters.Subscriber("ball_radius", Float32)
+    translate_cache = message_filters.Cache(translate_sub,10,allow_headerless=True)
+    translate_cache.registerCallback(translateCallback)
     try:
         rospy.spin()
     except KeyboardInterrupt: