Commit 1d766a0168306ee1c2ae4d83d82537e366ae24b0

Authored by Zhang Xuan
1 parent 26ea68cf

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

Showing 2 changed files with 48 additions and 14 deletions   Show diff stats
src/find_color.py
... ... @@ -21,12 +21,15 @@ def nothing(x):
21 21 pass
22 22  
23 23 class FindColor:
24   - def __init__(self, debug=True):
  24 + def __init__(self, debug=False):
25 25 self.bridge = CvBridge()
26 26 # Subscribe to the camera
27 27 self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
  28 +
28 29 # Create the center_deviation topic where we will publish the error
29 30 self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
  31 + # Create the ball_radius topic where we will publish the radius of the ball
  32 + self.ball_radius_pub = rospy.Publisher('ball_radius', Float32, queue_size = 10)
30 33 # Dummy images
31 34 self.cv_image = np.zeros((100, 100, 3), np.uint8)
32 35 self.hsv = np.zeros((100, 100, 3), np.uint8)
... ... @@ -34,12 +37,12 @@ class FindColor:
34 37 self.h_min, self.s_min, self.v_min = GREEN_LOWER
35 38 self.h_max, self.s_max, self.v_max = GREEN_UPPER
36 39 self.debug = debug
37   -
38 40 if debug:
39 41 self.image_name, self.mask_name = "RGB", "Mask"
40 42 # Create Windows
41 43 cv2.namedWindow(self.image_name)
42 44 cv2.namedWindow(self.mask_name)
  45 + cv2.waitKey(1000)
43 46 # Creating track bar
44 47 for min_max in ['min', 'max']:
45 48 cv2.createTrackbar('h_{}'.format(min_max), self.mask_name, H_MIN, H_MAX, nothing)
... ... @@ -60,7 +63,6 @@ class FindColor:
60 63 except CvBridgeError as e:
61 64 print("Error converting the image: {}".format(e))
62 65 return
63   -
64 66 if self.debug:
65 67 # Get info from track bar and appy to result
66 68 self.h_min = cv2.getTrackbarPos('h_min', self.mask_name)
... ... @@ -101,7 +103,7 @@ class FindColor:
101 103  
102 104 # Create the masked image
103 105 self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
104   -
  106 +
105 107 # Only proceed if the radius meets a minimum size
106 108 if radius > MIN_RADIUS_SIZE and self.debug:
107 109 # Draw the enclosing circle and center on the two images
... ... @@ -122,10 +124,14 @@ class FindColor:
122 124 # Publish the error to the corresponding topic
123 125 self.center_pub.publish(center_deviation)
124 126  
  127 + # Publish the ball_radius
  128 + if radius > MIN_RADIUS_SIZE:
  129 + self.ball_radius_pub.publish(radius)
  130 +
125 131 if self.debug:
126 132 # Show the images
127 133 cv2.imshow(self.image_name, self.cv_image)
128   - cv2.imshow(self.mask_name, self.mask_image)
  134 + cv2.imshow(self.masked, self.mask_image)
129 135 # Wait 3 ms to show the images
130 136 cv2.waitKey(3)
131 137  
... ... @@ -133,8 +139,8 @@ class FindColor:
133 139 if __name__ == '__main__':
134 140  
135 141 print("Starting FindColor node")
136   - ic = FindColor()
137 142 rospy.init_node('FindColor', anonymous=True)
  143 + ic = FindColor()
138 144 try:
139 145 rospy.spin()
140 146 except KeyboardInterrupt:
... ...
src/follow_color.py
... ... @@ -2,28 +2,31 @@
2 2 from __future__ import print_function, division
3 3  
4 4 import time
5   -
  5 +import message_filters
6 6 import rospy
7 7 import numpy as np
8 8 from geometry_msgs.msg import Twist
9 9 from std_msgs.msg import Float32
10 10  
11 11 vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
12   -Kp = 2
  12 +Kp_w = 2
13 13 Kd = 1
14 14 Ki = 0
15 15 last_time = None
16 16  
  17 +Kp_x = 0.2
  18 +
  19 +vel_x = 0
  20 +omega_z = 0
  21 +
17 22 def turnCallback(data):
18   - global errorD, errorI, last_time
  23 +
  24 + global errorD, errorI, last_time, omega_z
19 25 # Error (deviation to the center rescaled to [-1, 1])
20 26 error = data.data
21 27 # print("Error: {}".format(error))
22 28  
23   - # Init twist message (speed + orientation command)
24   - vel_msg = Twist()
25   - vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = 0, 0, 0
26   - vel_msg.angular.x, vel_msg.angular.y = 0, 0
  29 +
27 30 if last_time is not None:
28 31 errorD = error - last_error
29 32 dt = time.time() - last_time
... ... @@ -32,18 +35,43 @@ def turnCallback(data):
32 35 errorI, errorD = 0, 0
33 36 dt = 1
34 37 # PID Control
35   - vel_msg.angular.z = - (Kp * error + Kd * (errorD / dt) + Ki * dt * errorI)
  38 + omega_z = - (Kp_w * error + Kd * (errorD / dt) + Ki * dt * errorI)
36 39 # Update integral error
37 40 errorI += error
38 41  
  42 + # Init twist message (speed + orientation command)
  43 + vel_msg = Twist()
  44 + vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
  45 + vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z = 0, 0, omega_z
39 46 # Publish the Twist message
40 47 vel_pub.publish(vel_msg)
41 48  
  49 +def translateCallback(raw_data):
  50 + data=np.mean(raw_data.data)
  51 +
  52 + global vel_x
  53 + # Error (deviation to the center rescaled to [-1, 1])
  54 + error = (data - 45)/20
  55 + # print("Error: {}".format(error))
  56 +
  57 + # PID Control
  58 + vel_x = - Kp_x * error
  59 +
  60 +
  61 + # Init twist message (speed + orientation command)
  62 + vel_msg = Twist()
  63 + vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
  64 + vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z= 0, 0, omega_z
  65 + # Publish the Twist message
  66 + vel_pub.publish(vel_msg)
42 67  
43 68 if __name__ == "__main__":
44 69 print("Starting follow_color node")
45 70 rospy.init_node('follow_color', anonymous=True)
46 71 turn_sub = rospy.Subscriber("center_deviation", Float32, turnCallback)
  72 + translate_sub = message_filters.Subscriber("ball_radius", Float32)
  73 + translate_cache = message_filters.Cache(translate_sub,10,allow_headerless=True)
  74 + translate_cache.registerCallback(translateCallback)
47 75 try:
48 76 rospy.spin()
49 77 except KeyboardInterrupt:
... ...