Commit 74da89e5d670f9f6d7b13e9d97f06cc869e417b4

Authored by Zhang Xuan
1 parent 1d766a01

skelet for the tutorial

Showing 2 changed files with 13 additions and 60 deletions   Show diff stats
src/find_color.py
... ... @@ -22,12 +22,13 @@ def nothing(x):
22 22  
23 23 class FindColor:
24 24 def __init__(self, debug=False):
  25 + # Converter of ros image to opencv image
25 26 self.bridge = CvBridge()
26 27 # Subscribe to the camera
27   - self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
  28 + #TODO
28 29  
29 30 # Create the center_deviation topic where we will publish the error
30   - self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
  31 + #TODO
31 32 # Create the ball_radius topic where we will publish the radius of the ball
32 33 self.ball_radius_pub = rospy.Publisher('ball_radius', Float32, queue_size = 10)
33 34 # Dummy images
... ... @@ -121,13 +122,15 @@ class FindColor:
121 122 max_deviation = image_width // 2
122 123 center_deviation = (center[0] - max_deviation) / max_deviation
123 124 # print("Center deviation : {}".format(center_deviation))
  125 +
124 126 # Publish the error to the corresponding topic
125   - self.center_pub.publish(center_deviation)
  127 + #TODO
126 128  
127 129 # Publish the ball_radius
128 130 if radius > MIN_RADIUS_SIZE:
129 131 self.ball_radius_pub.publish(radius)
130 132  
  133 +
131 134 if self.debug:
132 135 # Show the images
133 136 cv2.imshow(self.image_name, self.cv_image)
... ...
src/follow_color.py
... ... @@ -8,70 +8,20 @@ import numpy as np
8 8 from geometry_msgs.msg import Twist
9 9 from std_msgs.msg import Float32
10 10  
11   -vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
12   -Kp_w = 2
13   -Kd = 1
14   -Ki = 0
15   -last_time = None
16   -
17   -Kp_x = 0.2
18   -
19   -vel_x = 0
20   -omega_z = 0
  11 +# Publisher of velocity
  12 +#TODO
21 13  
  14 +# callback of the Subscriber
22 15 def turnCallback(data):
  16 + #TODO
  17 + pass
23 18  
24   - global errorD, errorI, last_time, omega_z
25   - # Error (deviation to the center rescaled to [-1, 1])
26   - error = data.data
27   - # print("Error: {}".format(error))
28   -
29   -
30   - if last_time is not None:
31   - errorD = error - last_error
32   - dt = time.time() - last_time
33   - last_time = time.time()
34   - else:
35   - errorI, errorD = 0, 0
36   - dt = 1
37   - # PID Control
38   - omega_z = - (Kp_w * error + Kd * (errorD / dt) + Ki * dt * errorI)
39   - # Update integral error
40   - errorI += error
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
46   - # Publish the Twist message
47   - vel_pub.publish(vel_msg)
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)
67 19  
68 20 if __name__ == "__main__":
69 21 print("Starting follow_color node")
70 22 rospy.init_node('follow_color', anonymous=True)
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)
  23 + # Declare the Subscriber to center_deviationturn
  24 + #TODO
75 25 try:
76 26 rospy.spin()
77 27 except KeyboardInterrupt:
... ...